Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: RCOutput_PCA9685: group writes

Browse Source
This commit is contained in:
Lucas De Marchi 2015-09-28 19:39:17 -03:00 committed by Andrew Tridgell
parent f43f6c53f7
commit 24f41538cb
2 changed files with 53 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
View File

@ -181,26 +181,62 @@ void LinuxRCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
return;
}
_pulses_buffer[ch] = period_us;
_pending_write_mask |= (1U << ch);
if (!_corking)
push();
}
void LinuxRCOutput_PCA9685::cork()
{
_corking = true;
}
void LinuxRCOutput_PCA9685::push()
{
_corking = false;
if (_pending_write_mask == 0)
return;
// Calculate the number of channels for this transfer.
uint8_t max_ch = (sizeof(unsigned) * 8) - __builtin_clz(_pending_write_mask);
uint8_t min_ch = __builtin_ctz(_pending_write_mask);
/*
* scratch buffer size is always for all the channels, but we write only
* from min_ch to max_ch
*/
uint8_t data[PWM_CHAN_COUNT * 4] = { };
for (unsigned ch = min_ch; ch < max_ch; ch++) {
uint16_t period_us = _pulses_buffer[ch];
uint16_t length = 0;
if (period_us)
length = round((period_us * 4096) / (1000000.f / _frequency)) - 1;
uint8_t *d = &data[ch * 4];
*d++ = 0;
*d++ = 0;
*d++ = length & 0xFF;
*d++ = length >> 8;
}
if (!_i2c_sem->take_nonblocking()) {
hal.console->printf("RCOutput: Unable to get bus semaphore");
return;
}
uint16_t length;
if (period_us == 0)
length = 0;
else
length = round((period_us * 4096) / (1000000.f / _frequency)) - 1;
length = htole16(length);
hal.i2c->writeRegisters(_addr,
PCA9685_RA_LED0_OFF_L + 4 * (ch + _channel_offset),
2,
(uint8_t *)&length);
_pulses_buffer[ch] = period_us;
PCA9685_RA_LED0_ON_L + 4 * (_channel_offset + min_ch),
(max_ch - min_ch) * 4,
&data[min_ch * 4]);
_i2c_sem->give();
_pending_write_mask = 0;
}
uint16_t LinuxRCOutput_PCA9685::read(uint8_t ch)

4
libraries/AP_HAL_Linux/RCOutput_PCA9685.h
View File

@ -20,6 +20,8 @@ class Linux::LinuxRCOutput_PCA9685 : public AP_HAL::RCOutput {
void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);
void write(uint8_t ch, uint16_t period_us);
void cork() override;
void push() override;
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
@ -35,8 +37,10 @@ private:
uint8_t _addr;
bool _external_clock;
bool _corking = false;
uint8_t _channel_offset;
int16_t _oe_pin_number;
uint16_t _pending_write_mask;
};
#endif // __AP_HAL_LINUX_RCOUTPUT_PCA9685_H__