diff --git a/libraries/AP_HAL_Linux/RCOutput_Navio.cpp b/libraries/AP_HAL_Linux/RCOutput_Navio.cpp index e1acbaf305..333668c479 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Navio.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Navio.cpp @@ -1,7 +1,8 @@ #include +#include "GPIO.h" -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO #include "RCOutput_Navio.h" #include @@ -17,6 +18,7 @@ using namespace Linux; #define PWM_CHAN_COUNT 13 +#define PCA9685_OUTPUT_ENABLE RPI_GPIO_27 static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; @@ -28,7 +30,12 @@ void LinuxRCOutput_Navio::init(void* machtnicht) "valid I2C semaphore!")); return; // never reached } - + + /* Enable PCA9685 PWM */ + enable_pin = hal.gpio->channel(PCA9685_OUTPUT_ENABLE); + enable_pin->mode(HAL_GPIO_OUTPUT); + enable_pin->write(0); + // Set the initial frequency set_freq(0, 50); } diff --git a/libraries/AP_HAL_Linux/RCOutput_Navio.h b/libraries/AP_HAL_Linux/RCOutput_Navio.h index 2a97fbf9f2..9b9894d5d1 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Navio.h +++ b/libraries/AP_HAL_Linux/RCOutput_Navio.h @@ -47,6 +47,7 @@ class Linux::LinuxRCOutput_Navio : public AP_HAL::RCOutput { private: void reset(); AP_HAL::Semaphore *_i2c_sem; + AP_HAL::DigitalSource *enable_pin; uint16_t _frequency; };