From 3e765e23a9ed30b382a1cd94222e3591627dbdf7 Mon Sep 17 00:00:00 2001 From: Luiz Ywata Date: Fri, 27 May 2016 09:35:59 -0300 Subject: [PATCH] AP_HAL_Linux: RCOutput_PCA9685: use I2CDevice interface --- libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 14 +++-- libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp | 68 ++++++++++----------- libraries/AP_HAL_Linux/RCOutput_PCA9685.h | 12 ++-- 3 files changed, 48 insertions(+), 46 deletions(-) diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index b5366d8d20..b4ba43564f 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -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 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 diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp index 067b6b4dbb..469d905926 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include @@ -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 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; } diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.h b/libraries/AP_HAL_Linux/RCOutput_PCA9685.h index 095f7ad475..7199ada4fa 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.h +++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.h @@ -1,5 +1,7 @@ #pragma once +#include + #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 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 _dev; uint16_t _frequency; float _osc_clock; uint16_t *_pulses_buffer; - uint8_t _addr; bool _external_clock; bool _corking = false; uint8_t _channel_offset;