Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: RCOutput_Navio: allow to use internal clock

Browse Source 
It's possible to use the internal clock in PCA96895 if we account for
the drift it contains. This is a bit different from solutions in other
projects like the Adafruit library and the PX4 firmware: instead of
applying a correction to the final frequency we apply the correction to
the clock since this is the source of the error.

With this fix we arrived to much better results across different lots of
sensors.

The Navio board continues to use the external clock and should have no
difference behavior.
This commit is contained in:
Fabio Mello 2015-06-12 14:46:01 -03:00 committed by Andrew Tridgell
parent 6c042b6666
commit c0caed1b64
3 changed files with 32 additions and 9 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

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

35
libraries/AP_HAL_Linux/RCOutput_Navio.cpp
View File

@ -44,21 +44,35 @@
#define PCA9685_MODE2_OUTNE1_BIT (1 << 1)
#define PCA9685_MODE2_OUTNE0_BIT (1 << 0)
/*
* Drift for internal oscillator
* see: https://github.com/diydrones/ardupilot/commit/50459bdca0b5a1adf95
* and https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11
*/
#define PCA9685_INTERNAL_CLOCK (1.04f * 25000000.f)
#define PCA9685_EXTERNAL_CLOCK 24576000.f
using namespace Linux;
#define PWM_CHAN_COUNT 13
static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
LinuxRCOutput_Navio::LinuxRCOutput_Navio(uint8_t channel_offset,
LinuxRCOutput_Navio::LinuxRCOutput_Navio(bool external_clock,
uint8_t channel_offset,
uint8_t oe_pin_number) :
_i2c_sem(NULL),
_enable_pin(NULL),
_frequency(50),
_pulses_buffer(new uint16_t[PWM_CHAN_COUNT]),
_external_clock(external_clock),
_channel_offset(channel_offset),
_oe_pin_number(oe_pin_number)
{
if (_external_clock)
_osc_clock = PCA9685_EXTERNAL_CLOCK;
else
_osc_clock = PCA9685_INTERNAL_CLOCK;
}
LinuxRCOutput_Navio::~LinuxRCOutput_Navio()
@ -121,18 +135,25 @@ void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz)
/* Put PCA9685 to sleep (required to write prescaler) */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT);
/* Calculate and write prescale value to match frequency */
uint8_t prescale = round(24576000.f / 4096.f / freq_hz) - 1;
/* Calculate prescale and save frequency using this value: it may be
* different from @freq_hz due to rounding/ceiling. We use ceil() rather
* than round() so the resulting frequency is never greater than @freq_hz
*/
uint8_t prescale = ceil(_osc_clock / (4096 * freq_hz)) - 1;
_frequency = _osc_clock / (4096 * (prescale + 1));
/* Write prescale value to match frequency */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_PRE_SCALE, prescale);
/* Enable external clocking */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1,
PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT);
if (_external_clock) {
/* Enable external clocking */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1,
PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT);
}
/* Restart the device to apply new settings and enable auto-incremented write */
hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1,
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT);
_frequency = freq_hz;
_i2c_sem->give();
}

4
libraries/AP_HAL_Linux/RCOutput_Navio.h
View File

@ -6,7 +6,7 @@
class Linux::LinuxRCOutput_Navio : public AP_HAL::RCOutput {
public:
LinuxRCOutput_Navio(uint8_t channel_offset,
LinuxRCOutput_Navio(bool external_clock, uint8_t channel_offset,
uint8_t oe_pin_number);
~LinuxRCOutput_Navio();
void init(void* machtnichts);
@ -26,9 +26,11 @@ private:
AP_HAL::Semaphore *_i2c_sem;
AP_HAL::DigitalSource *_enable_pin;
uint16_t _frequency;
float _osc_clock;
uint16_t *_pulses_buffer;
bool _external_clock;
uint8_t _channel_offset;
uint8_t _oe_pin_number;
};