AP_HAL_Linux: enable output for PCA9685 PWM in NavioRCOuput

This commit is contained in:
Staroselskii Georgii 2014-11-26 12:37:07 +03:00 committed by Andrew Tridgell
parent c9b7d27b98
commit 2b589d4604
2 changed files with 10 additions and 2 deletions

View File

@ -1,7 +1,8 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#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 "RCOutput_Navio.h"
#include <sys/types.h> #include <sys/types.h>
@ -17,6 +18,7 @@
using namespace Linux; using namespace Linux;
#define PWM_CHAN_COUNT 13 #define PWM_CHAN_COUNT 13
#define PCA9685_OUTPUT_ENABLE RPI_GPIO_27
static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
@ -28,7 +30,12 @@ void LinuxRCOutput_Navio::init(void* machtnicht)
"valid I2C semaphore!")); "valid I2C semaphore!"));
return; // never reached 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 the initial frequency
set_freq(0, 50); set_freq(0, 50);
} }

View File

@ -47,6 +47,7 @@ class Linux::LinuxRCOutput_Navio : public AP_HAL::RCOutput {
private: private:
void reset(); void reset();
AP_HAL::Semaphore *_i2c_sem; AP_HAL::Semaphore *_i2c_sem;
AP_HAL::DigitalSource *enable_pin;
uint16_t _frequency; uint16_t _frequency;
}; };