AP_HAL_Linux: Only use enable pin for PCA9685 if we need to.

This commit is contained in:
Fredrik Hedberg 2015-09-08 08:33:34 +02:00 committed by Andrew Tridgell
parent 21b9f96ce3
commit 9d9e6d0b34
2 changed files with 9 additions and 6 deletions

View File

@ -60,7 +60,7 @@ static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
LinuxRCOutput_PCA9685::LinuxRCOutput_PCA9685(bool external_clock,
uint8_t channel_offset,
uint8_t oe_pin_number) :
int16_t oe_pin_number) :
_i2c_sem(NULL),
_enable_pin(NULL),
_frequency(50),
@ -95,9 +95,11 @@ void LinuxRCOutput_PCA9685::init(void* machtnicht)
set_freq(0, 50);
/* Enable PCA9685 PWM */
_enable_pin = hal.gpio->channel(_oe_pin_number);
_enable_pin->mode(HAL_GPIO_OUTPUT);
_enable_pin->write(0);
if (_oe_pin_number != -1) {
_enable_pin = hal.gpio->channel(_oe_pin_number);
_enable_pin->mode(HAL_GPIO_OUTPUT);
_enable_pin->write(0);
}
}
void LinuxRCOutput_PCA9685::reset_all_channels()

View File

@ -6,8 +6,9 @@
class Linux::LinuxRCOutput_PCA9685 : public AP_HAL::RCOutput {
public:
LinuxRCOutput_PCA9685(bool external_clock, uint8_t channel_offset,
uint8_t oe_pin_number);
int16_t oe_pin_number);
~LinuxRCOutput_PCA9685();
void init(void* machtnichts);
void reset_all_channels();
@ -32,7 +33,7 @@ private:
bool _external_clock;
uint8_t _channel_offset;
uint8_t _oe_pin_number;
int16_t _oe_pin_number;
};
#endif // __AP_HAL_LINUX_RCOUTPUT_PCA9685_H__