aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_Linux: RCOutput_PCA9685: use I2CDevice interface

Browse Source
This commit is contained in:
Luiz Ywata 2016-05-27 09:35:59 -03:00 committed by Lucas De Marchi
parent d5449a6f20
commit 3e765e23a9
3 changed files with 48 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -52,11 +52,13 @@ static I2CDriver i2cDriver2(2);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
static I2CDriver i2cDriver0(2);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
static I2CDriver i2cDriver0({
static const std::vector<const char *> i2c_devpaths({
/* UEFI with lpss set to ACPI */
"platform/80860F41:05",
/* UEFI with lpss set to PCI */
"pci0000:00/0000:00:18.6" });
"pci0000:00/0000:00:18.6",
});
static I2CDriver i2cDriver0(i2c_devpaths);
/* One additional emulated bus */
static I2CDriver i2cDriver1(10);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
@ -157,11 +159,11 @@ static RCOutput_AioPRU rcoutDriver;
*/
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
static RCOutput_PCA9685 rcoutDriver(PCA9685_PRIMARY_ADDRESS, true, 3, RPI_GPIO_27);
static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMARY_ADDRESS), true, 3, RPI_GPIO_27);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
static RCOutput_PCA9685 rcoutDriver(PCA9685_QUATENARY_ADDRESS, false, 0, RPI_GPIO_4);
static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_QUATENARY_ADDRESS), false, 0, RPI_GPIO_4);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
static RCOutput_PCA9685 rcoutDriver(PCA9685_PRIMARY_ADDRESS, true, 3, RPI_GPIO_27);
static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMARY_ADDRESS), true, 3, RPI_GPIO_27);
/*
use the STM32 based RCOutput driver on Raspilot
*/
@ -172,7 +174,7 @@ static RCOutput_ZYNQ rcoutDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
static RCOutput_Bebop rcoutDriver(i2c_mgr_instance.get_device(HAL_RCOUT_BEBOP_BLDC_I2C_BUS, HAL_RCOUT_BEBOP_BLDC_I2C_ADDR));
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
static RCOutput_PCA9685 rcoutDriver(PCA9685_PRIMARY_ADDRESS, false, 0, MINNOW_GPIO_S5_1);
static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(i2c_devpaths, PCA9685_PRIMARY_ADDRESS), false, 0, MINNOW_GPIO_S5_1);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
static RCOutput_QFLIGHT rcoutDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2

68
libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp
View File

@ -9,6 +9,7 @@
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <utility>
#include <AP_HAL/AP_HAL.h>
@ -55,15 +56,14 @@ using namespace Linux;
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),
RCOutput_PCA9685::RCOutput_PCA9685(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool external_clock,
uint8_t channel_offset,
int16_t oe_pin_number) :
_dev(std::move(dev)),
_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)
@ -81,13 +81,6 @@ RCOutput_PCA9685::~RCOutput_PCA9685()
void RCOutput_PCA9685::init()
{
_i2c_sem = hal.i2c->get_semaphore();
if (_i2c_sem == NULL) {
AP_HAL::panic("PANIC: RCOutput_PCA9685 did not get "
"valid I2C semaphore!");
return; /* never reached */
}
reset_all_channels();
/* Set the initial frequency */
@ -103,17 +96,17 @@ void RCOutput_PCA9685::init()
void RCOutput_PCA9685::reset_all_channels()
{
if (!_i2c_sem->take(10)) {
if (!_dev->get_semaphore()->take(10)) {
return;
}
uint8_t data[4] = {0x00, 0x00, 0x00, 0x00};
hal.i2c->writeRegisters(_addr, PCA9685_RA_ALL_LED_ON_L, 4, data);
uint8_t data[] = {PCA9685_RA_ALL_LED_ON_L, 0, 0, 0, 0};
_dev->transfer(data, sizeof(data), nullptr, 0);
/* Wait for the last pulse to end */
hal.scheduler->delay(2);
_i2c_sem->give();
_dev->get_semaphore()->give();
}
void RCOutput_PCA9685::set_freq(uint32_t chmask, uint16_t freq_hz)
@ -124,17 +117,17 @@ void RCOutput_PCA9685::set_freq(uint32_t chmask, uint16_t freq_hz)
write(i, _pulses_buffer[i]);
}
if (!_i2c_sem->take(10)) {
if (!_dev->get_semaphore()->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);
_dev->write_register(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);
_dev->write_register(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
@ -144,19 +137,19 @@ void RCOutput_PCA9685::set_freq(uint32_t chmask, uint16_t freq_hz)
_frequency = _osc_clock / (4096 * (prescale + 1));
/* Write prescale value to match frequency */
hal.i2c->writeRegister(_addr, PCA9685_RA_PRE_SCALE, prescale);
_dev->write_register(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);
_dev->write_register(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);
_dev->write_register(PCA9685_RA_MODE1,
PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT);
_i2c_sem->give();
_dev->get_semaphore()->give();
}
uint16_t RCOutput_PCA9685::get_freq(uint8_t ch)
@ -207,33 +200,36 @@ void RCOutput_PCA9685::push()
* 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] = { };
struct PACKED pwm_values {
uint8_t reg;
uint8_t data[PWM_CHAN_COUNT * 4];
} pwm_values;
for (unsigned ch = min_ch; ch < max_ch; ch++) {
uint16_t period_us = _pulses_buffer[ch];
uint16_t length = 0;
if (period_us)
if (period_us) {
length = round((period_us * 4096) / (1000000.f / _frequency)) - 1;
}
uint8_t *d = &data[ch * 4];
uint8_t *d = &pwm_values.data[(ch - min_ch) * 4];
*d++ = 0;
*d++ = 0;
*d++ = length & 0xFF;
*d++ = length >> 8;
}
if (!_i2c_sem->take_nonblocking()) {
if (!_dev->get_semaphore()->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();
pwm_values.reg = PCA9685_RA_LED0_ON_L + 4 * (_channel_offset + min_ch);
/* reg + all the channels we are going to write */
size_t payload_size = 1 + (max_ch - min_ch) * 4;
_dev->transfer((uint8_t *)&pwm_values, payload_size, nullptr, 0);
_dev->get_semaphore()->give();
_pending_write_mask = 0;
}

12
libraries/AP_HAL_Linux/RCOutput_PCA9685.h
View File

@ -1,5 +1,7 @@
#pragma once
#include <AP_HAL/I2CDevice.h>
#include "AP_HAL_Linux.h"
#define PCA9685_PRIMARY_ADDRESS 0x40 // All address pins low, PCA9685 default
@ -9,8 +11,11 @@
class Linux::RCOutput_PCA9685 : public AP_HAL::RCOutput {
public:
RCOutput_PCA9685(uint8_t addr, bool external_clock, uint8_t channel_offset,
int16_t oe_pin_number);
RCOutput_PCA9685(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
bool external_clock,
uint8_t channel_offset,
int16_t oe_pin_number);
~RCOutput_PCA9685();
void init();
void reset_all_channels();
@ -27,14 +32,13 @@ class Linux::RCOutput_PCA9685 : public AP_HAL::RCOutput {
private:
void reset();
AP_HAL::Semaphore *_i2c_sem;
AP_HAL::DigitalSource *_enable_pin;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint16_t _frequency;
float _osc_clock;
uint16_t *_pulses_buffer;
uint8_t _addr;
bool _external_clock;
bool _corking = false;
uint8_t _channel_offset;