mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
6984168a45
With commit 24f4153 ("AP_HAL_Linux: RCOutput_PCA9685: group writes") a
log was introduced when we can't get the bus semaphore. However since we
are calling the non blocking method, failing there is not that unlikely
if the bus is shared. Return back to the previous behavior of not
logging.
253 lines
6.6 KiB
C++
253 lines
6.6 KiB
C++
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "GPIO.h"
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
|
|
|
#include "RCOutput_PCA9685.h"
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <dirent.h>
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <math.h>
|
|
|
|
#define PCA9685_RA_MODE1 0x00
|
|
#define PCA9685_RA_MODE2 0x01
|
|
#define PCA9685_RA_LED0_ON_L 0x06
|
|
#define PCA9685_RA_LED0_ON_H 0x07
|
|
#define PCA9685_RA_LED0_OFF_L 0x08
|
|
#define PCA9685_RA_LED0_OFF_H 0x09
|
|
#define PCA9685_RA_ALL_LED_ON_L 0xFA
|
|
#define PCA9685_RA_ALL_LED_ON_H 0xFB
|
|
#define PCA9685_RA_ALL_LED_OFF_L 0xFC
|
|
#define PCA9685_RA_ALL_LED_OFF_H 0xFD
|
|
#define PCA9685_RA_PRE_SCALE 0xFE
|
|
|
|
#define PCA9685_MODE1_RESTART_BIT (1 << 7)
|
|
#define PCA9685_MODE1_EXTCLK_BIT (1 << 6)
|
|
#define PCA9685_MODE1_AI_BIT (1 << 5)
|
|
#define PCA9685_MODE1_SLEEP_BIT (1 << 4)
|
|
#define PCA9685_MODE1_SUB1_BIT (1 << 3)
|
|
#define PCA9685_MODE1_SUB2_BIT (1 << 2)
|
|
#define PCA9685_MODE1_SUB3_BIT (1 << 1)
|
|
#define PCA9685_MODE1_ALLCALL_BIT (1 << 0)
|
|
#define PCA9685_ALL_LED_OFF_H_SHUT (1 << 4)
|
|
#define PCA9685_MODE2_INVRT_BIT (1 << 4)
|
|
#define PCA9685_MODE2_OCH_BIT (1 << 3)
|
|
#define PCA9685_MODE2_OUTDRV_BIT (1 << 2)
|
|
#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 16
|
|
|
|
static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|
|
|
RCOutput_PCA9685::RCOutput_PCA9685(uint8_t addr,
|
|
bool external_clock,
|
|
uint8_t channel_offset,
|
|
int16_t oe_pin_number) :
|
|
_i2c_sem(NULL),
|
|
_enable_pin(NULL),
|
|
_frequency(50),
|
|
_pulses_buffer(new uint16_t[PWM_CHAN_COUNT - channel_offset]),
|
|
_addr(addr),
|
|
_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;
|
|
}
|
|
|
|
RCOutput_PCA9685::~RCOutput_PCA9685()
|
|
{
|
|
delete [] _pulses_buffer;
|
|
}
|
|
|
|
void RCOutput_PCA9685::init(void* machtnicht)
|
|
{
|
|
_i2c_sem = hal.i2c->get_semaphore();
|
|
if (_i2c_sem == NULL) {
|
|
hal.scheduler->panic("PANIC: RCOutput_PCA9685 did not get "
|
|
"valid I2C semaphore!");
|
|
return; /* never reached */
|
|
}
|
|
|
|
reset_all_channels();
|
|
|
|
/* Set the initial frequency */
|
|
set_freq(0, 50);
|
|
|
|
/* Enable PCA9685 PWM */
|
|
if (_oe_pin_number != -1) {
|
|
_enable_pin = hal.gpio->channel(_oe_pin_number);
|
|
_enable_pin->mode(HAL_GPIO_OUTPUT);
|
|
_enable_pin->write(0);
|
|
}
|
|
}
|
|
|
|
void RCOutput_PCA9685::reset_all_channels()
|
|
{
|
|
if (!_i2c_sem->take(10)) {
|
|
return;
|
|
}
|
|
|
|
uint8_t data[4] = {0x00, 0x00, 0x00, 0x00};
|
|
hal.i2c->writeRegisters(_addr, PCA9685_RA_ALL_LED_ON_L, 4, data);
|
|
|
|
/* Wait for the last pulse to end */
|
|
hal.scheduler->delay(2);
|
|
|
|
_i2c_sem->give();
|
|
}
|
|
|
|
void RCOutput_PCA9685::set_freq(uint32_t chmask, uint16_t freq_hz)
|
|
{
|
|
|
|
/* Correctly finish last pulses */
|
|
for (int i = 0; i < (PWM_CHAN_COUNT - _channel_offset); i++) {
|
|
write(i, _pulses_buffer[i]);
|
|
}
|
|
|
|
if (!_i2c_sem->take(10)) {
|
|
return;
|
|
}
|
|
|
|
/* Shutdown before sleeping.
|
|
* see p.14 of PCA9685 product datasheet
|
|
*/
|
|
hal.i2c->writeRegister(_addr, PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT);
|
|
|
|
/* Put PCA9685 to sleep (required to write prescaler) */
|
|
hal.i2c->writeRegister(_addr, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT);
|
|
|
|
/* 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(_addr, PCA9685_RA_PRE_SCALE, prescale);
|
|
|
|
if (_external_clock) {
|
|
/* Enable external clocking */
|
|
hal.i2c->writeRegister(_addr, 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(_addr, PCA9685_RA_MODE1,
|
|
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT);
|
|
|
|
_i2c_sem->give();
|
|
}
|
|
|
|
uint16_t RCOutput_PCA9685::get_freq(uint8_t ch)
|
|
{
|
|
return _frequency;
|
|
}
|
|
|
|
void RCOutput_PCA9685::enable_ch(uint8_t ch)
|
|
{
|
|
|
|
}
|
|
|
|
void RCOutput_PCA9685::disable_ch(uint8_t ch)
|
|
{
|
|
write(ch, 0);
|
|
}
|
|
|
|
void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
|
|
{
|
|
if (ch >= (PWM_CHAN_COUNT - _channel_offset)) {
|
|
return;
|
|
}
|
|
|
|
_pulses_buffer[ch] = period_us;
|
|
_pending_write_mask |= (1U << ch);
|
|
|
|
if (!_corking)
|
|
push();
|
|
}
|
|
|
|
void RCOutput_PCA9685::cork()
|
|
{
|
|
_corking = true;
|
|
}
|
|
|
|
void RCOutput_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()) {
|
|
return;
|
|
}
|
|
|
|
hal.i2c->writeRegisters(_addr,
|
|
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 RCOutput_PCA9685::read(uint8_t ch)
|
|
{
|
|
return _pulses_buffer[ch];
|
|
}
|
|
|
|
void RCOutput_PCA9685::read(uint16_t* period_us, uint8_t len)
|
|
{
|
|
for (int i = 0; i < len; i++)
|
|
period_us[i] = read(0 + i);
|
|
}
|
|
|
|
#endif
|