Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: Allow PCA9685 to be used on non-default I2C addresses.

This commit is contained in:
Fredrik Hedberg 2015-09-08 08:39:12 +02:00 committed by Andrew Tridgell
parent 9d9e6d0b34
commit d9a4b6b089
3 changed files with 17 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -96,7 +96,7 @@ static LinuxRCOutput_AioPRU rcoutDriver;
use the PCA9685 based RCOutput driver on Navio use the PCA9685 based RCOutput driver on Navio
*/ */
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
static LinuxRCOutput_PCA9685 rcoutDriver(true, 3, RPI_GPIO_27); static LinuxRCOutput_PCA9685 rcoutDriver(PCA9685_PRIMARY_ADDRESS, true, 3, RPI_GPIO_27);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ
static LinuxRCOutput_ZYNQ rcoutDriver; static LinuxRCOutput_ZYNQ rcoutDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP

20
libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
View File

@ -15,8 +15,6 @@
#include <stdint.h> #include <stdint.h>
#include <math.h> #include <math.h>
#define PCA9685_ADDRESS 0x40 // All address pins low, Navio default
#define PCA9685_RA_MODE1 0x00 #define PCA9685_RA_MODE1 0x00
#define PCA9685_RA_MODE2 0x01 #define PCA9685_RA_MODE2 0x01
#define PCA9685_RA_LED0_ON_L 0x06 #define PCA9685_RA_LED0_ON_L 0x06
@ -58,13 +56,15 @@ using namespace Linux;
static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
LinuxRCOutput_PCA9685::LinuxRCOutput_PCA9685(bool external_clock, LinuxRCOutput_PCA9685::LinuxRCOutput_PCA9685(uint8_t addr,
bool external_clock,
uint8_t channel_offset, uint8_t channel_offset,
int16_t oe_pin_number) : int16_t oe_pin_number) :
_i2c_sem(NULL), _i2c_sem(NULL),
_enable_pin(NULL), _enable_pin(NULL),
_frequency(50), _frequency(50),
_pulses_buffer(new uint16_t[PWM_CHAN_COUNT - channel_offset]), _pulses_buffer(new uint16_t[PWM_CHAN_COUNT - channel_offset]),
_addr(addr),
_external_clock(external_clock), _external_clock(external_clock),
_channel_offset(channel_offset), _channel_offset(channel_offset),
_oe_pin_number(oe_pin_number) _oe_pin_number(oe_pin_number)
@ -109,7 +109,7 @@ void LinuxRCOutput_PCA9685::reset_all_channels()
} }
uint8_t data[4] = {0x00, 0x00, 0x00, 0x00}; uint8_t data[4] = {0x00, 0x00, 0x00, 0x00};
hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_RA_ALL_LED_ON_L, 4, data); hal.i2c->writeRegisters(_addr, PCA9685_RA_ALL_LED_ON_L, 4, data);
/* Wait for the last pulse to end */ /* Wait for the last pulse to end */
hal.scheduler->delay(2); hal.scheduler->delay(2);
@ -132,10 +132,10 @@ void LinuxRCOutput_PCA9685::set_freq(uint32_t chmask, uint16_t freq_hz)
/* Shutdown before sleeping. /* Shutdown before sleeping.
* see p.14 of PCA9685 product datasheet * see p.14 of PCA9685 product datasheet
*/ */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT); hal.i2c->writeRegister(_addr, PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT);
/* Put PCA9685 to sleep (required to write prescaler) */ /* Put PCA9685 to sleep (required to write prescaler) */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT); hal.i2c->writeRegister(_addr, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT);
/* Calculate prescale and save frequency using this value: it may be /* Calculate prescale and save frequency using this value: it may be
* different from @freq_hz due to rounding/ceiling. We use ceil() rather * different from @freq_hz due to rounding/ceiling. We use ceil() rather
@ -145,16 +145,16 @@ void LinuxRCOutput_PCA9685::set_freq(uint32_t chmask, uint16_t freq_hz)
_frequency = _osc_clock / (4096 * (prescale + 1)); _frequency = _osc_clock / (4096 * (prescale + 1));
/* Write prescale value to match frequency */ /* Write prescale value to match frequency */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_PRE_SCALE, prescale); hal.i2c->writeRegister(_addr, PCA9685_RA_PRE_SCALE, prescale);
if (_external_clock) { if (_external_clock) {
/* Enable external clocking */ /* Enable external clocking */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, hal.i2c->writeRegister(_addr, PCA9685_RA_MODE1,
PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT); PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT);
} }
/* Restart the device to apply new settings and enable auto-incremented write */ /* Restart the device to apply new settings and enable auto-incremented write */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, hal.i2c->writeRegister(_addr, PCA9685_RA_MODE1,
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT); PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT);
_i2c_sem->give(); _i2c_sem->give();
@ -193,7 +193,7 @@ void LinuxRCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
length = round((period_us * 4096) / (1000000.f / _frequency)) - 1; length = round((period_us * 4096) / (1000000.f / _frequency)) - 1;
uint8_t data[2] = {length & 0xFF, length >> 8}; uint8_t data[2] = {length & 0xFF, length >> 8};
uint8_t status = hal.i2c->writeRegisters(PCA9685_ADDRESS, uint8_t status = hal.i2c->writeRegisters(_addr,
PCA9685_RA_LED0_OFF_L + 4 * (ch + _channel_offset), PCA9685_RA_LED0_OFF_L + 4 * (ch + _channel_offset),
2, 2,
data); data);

8
libraries/AP_HAL_Linux/RCOutput_PCA9685.h
View File

@ -4,10 +4,13 @@
#include "AP_HAL_Linux.h" #include "AP_HAL_Linux.h"
#define PCA9685_PRIMARY_ADDRESS 0x40 // All address pins low, PCA9685 default
#define PCA9685_SECONDARY_ADDRESS 0x41
#define PCA9685_TERTIARY_ADDRESS 0x42
class Linux::LinuxRCOutput_PCA9685 : public AP_HAL::RCOutput { class Linux::LinuxRCOutput_PCA9685 : public AP_HAL::RCOutput {
public: public:
LinuxRCOutput_PCA9685(uint8_t addr, bool external_clock, uint8_t channel_offset,
LinuxRCOutput_PCA9685(bool external_clock, uint8_t channel_offset,
int16_t oe_pin_number); int16_t oe_pin_number);
~LinuxRCOutput_PCA9685(); ~LinuxRCOutput_PCA9685();
void init(void* machtnichts); void init(void* machtnichts);
@ -31,6 +34,7 @@ private:
uint16_t *_pulses_buffer; uint16_t *_pulses_buffer;
uint8_t _addr;
bool _external_clock; bool _external_clock;
uint8_t _channel_offset; uint8_t _channel_offset;
int16_t _oe_pin_number; int16_t _oe_pin_number;