From 955753f3f058412684a914f307e51ea808aa33f4 Mon Sep 17 00:00:00 2001 From: Mikhail Avkhimenia Date: Mon, 17 Nov 2014 13:14:51 +0300 Subject: [PATCH] HAL_Linux: Optimize bandwidth for RCOutput_Navio 40% less bytes in I2C transactions for PCA9685. --- libraries/AP_HAL_Linux/RCOutput_Navio.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCOutput_Navio.cpp b/libraries/AP_HAL_Linux/RCOutput_Navio.cpp index b204d541ff..e1acbaf305 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Navio.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Navio.cpp @@ -46,6 +46,10 @@ void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz) uint8_t prescale = round(24576000.f / 4096.f / freq_hz) - 1; hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_PRE_SCALE, prescale); + // Reset all channels + uint8_t data[4] = {0x00, 0x00, 0x00, 0x00}; + hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_RA_ALL_LED_ON_L, 4, data); + // Enable external clocking hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT | PCA9685_MODE1_EXTCLK_BIT); @@ -90,10 +94,10 @@ void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t period_us) else length = round((period_us * 4096) / (1000000.f / _frequency)) - 1; - uint8_t data[4] = {0x00, 0x00, length & 0xFF, length >> 8}; + uint8_t data[2] = {length & 0xFF, length >> 8}; uint8_t status = hal.i2c->writeRegisters(PCA9685_ADDRESS, - PCA9685_RA_LED0_ON_L + 4 * (ch + 3), - 4, + PCA9685_RA_LED0_OFF_L + 4 * (ch + 3), + 2, data); _i2c_sem->give();