From 7fb5db8077287dd48c82f3801397ac2c7612eff8 Mon Sep 17 00:00:00 2001 From: Luiz Ywata Date: Thu, 14 Apr 2016 17:40:45 -0300 Subject: [PATCH] AP_HAL_Linux: RCOutput_Bebop: use I2CDevice interface --- libraries/AP_HAL/AP_HAL_Boards.h | 1 + libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 2 +- libraries/AP_HAL_Linux/RCOutput_Bebop.cpp | 70 +++++++++------------- libraries/AP_HAL_Linux/RCOutput_Bebop.h | 5 +- 4 files changed, 34 insertions(+), 44 deletions(-) diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 51890cebe7..127d146908 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -231,6 +231,7 @@ #define HAL_FLOW_PX4_BOTTOM_FLOW_FEATURE_THRESHOLD 30 #define HAL_FLOW_PX4_BOTTOM_FLOW_VALUE_THRESHOLD 5000 #define HAL_PARAM_DEFAULTS_PATH "/etc/arducopter/bebop.parm" +#define HAL_RCOUT_BEBOP_BLDC_I2C_ADDR 0x08 /* focal length 3.6 um, 2x binning in each direction * 240x240 crop rescaled to 64x64 */ #define HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX (2.5 / (3.6 * 2.0 * 240 / 64)) diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index ec58437b50..d546457d1d 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -170,7 +170,7 @@ static RCOutput_Raspilot rcoutDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ static RCOutput_ZYNQ rcoutDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP -static RCOutput_Bebop rcoutDriver; +static RCOutput_Bebop rcoutDriver(i2c_mgr_instance.get_device(1, 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); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp index 54ac985964..65cdf05827 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp @@ -14,9 +14,10 @@ #include "RCOutput_Bebop.h" #include "Util.h" +#include + +/* BEBOP BLDC registers description */ -/* BEBOP BLDC motor controller address and registers description */ -#define BEBOP_BLDC_I2C_ADDR 0x08 #define BEBOP_BLDC_STARTPROP 0x40 #define BEBOP_BLDC_SETREFSPEED 0x02 @@ -74,8 +75,8 @@ using namespace Linux; static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); -RCOutput_Bebop::RCOutput_Bebop(): - _i2c_sem(NULL), +RCOutput_Bebop::RCOutput_Bebop(AP_HAL::OwnPtr dev): + _dev(std::move(dev)), _min_pwm(BEBOP_BLDC_MIN_PERIOD_US), _max_pwm(BEBOP_BLDC_MAX_PERIOD_US), _state(BEBOP_BLDC_STOPPED) @@ -100,12 +101,12 @@ void RCOutput_Bebop::_start_prop() { uint8_t data = BEBOP_BLDC_STARTPROP; - if (!_i2c_sem->take(0)) + if (!_dev->get_semaphore()->take(0)) return; - hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, 1, &data); + _dev->transfer(&data, sizeof(data), nullptr, 0); - _i2c_sem->give(); + _dev->get_semaphore()->give(); _state = BEBOP_BLDC_STARTED; } @@ -122,12 +123,12 @@ void RCOutput_Bebop::_set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM]) data.enable_security = 0; data.checksum = _checksum((uint8_t *) &data, sizeof(data) - 1); - if (!_i2c_sem->take(0)) + if (!_dev->get_semaphore()->take(0)) return; - hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, sizeof(data), (uint8_t *)&data); + _dev->transfer((uint8_t *)&data, sizeof(data), nullptr, 0); - _i2c_sem->give(); + _dev->get_semaphore()->give(); } bool RCOutput_Bebop::_get_info(struct bldc_info *info) @@ -136,12 +137,11 @@ bool RCOutput_Bebop::_get_info(struct bldc_info *info) return false; } memset(info, 0, sizeof(struct bldc_info)); - if (!_i2c_sem->take(0)) { + if (!_dev->get_semaphore()->take(0)) { return false; } - hal.i2c1->readRegisters(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_GET_INFO, - sizeof(struct bldc_info), (uint8_t *)info); - _i2c_sem->give(); + _dev->read_registers(BEBOP_BLDC_GET_INFO, (uint8_t*)info, sizeof(*info)); + _dev->get_semaphore()->give(); return true; } @@ -151,13 +151,11 @@ int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs) int i; memset(&data, 0, sizeof(data)); - if (!_i2c_sem->take(0)) + if (!_dev->get_semaphore()->take(0)) return -EBUSY; - hal.i2c1->readRegisters(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_GETOBSDATA, - sizeof(data), (uint8_t *)&data); - - _i2c_sem->give(); + _dev->read_registers(BEBOP_BLDC_GETOBSDATA, (uint8_t *)&data, sizeof(data)); + _dev->get_semaphore()->give(); if (data.checksum != _checksum((uint8_t *)&data, sizeof(data) - 1)) hal.console->printf("RCOutput_Bebop: bad checksum in obs data"); @@ -183,12 +181,11 @@ int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs) void RCOutput_Bebop::_toggle_gpio(uint8_t mask) { - if (!_i2c_sem->take(0)) + if (!_dev->get_semaphore()->take(0)) return; - hal.i2c1->writeRegister(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_TOGGLE_GPIO, mask); - - _i2c_sem->give(); + _dev->write_register(BEBOP_BLDC_TOGGLE_GPIO, mask); + _dev->get_semaphore()->give(); } void RCOutput_Bebop::_stop_prop() @@ -196,34 +193,31 @@ void RCOutput_Bebop::_stop_prop() uint8_t data = BEBOP_BLDC_STOP_PROP; _state = BEBOP_BLDC_STOPPED; - if (!_i2c_sem->take(0)) + if (!_dev->get_semaphore()->take(0)) return; - hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, 1, &data); - - _i2c_sem->give(); + _dev->transfer(&data, sizeof(data), nullptr, 0); + _dev->get_semaphore()->give(); } void RCOutput_Bebop::_clear_error() { uint8_t data = BEBOP_BLDC_CLEAR_ERROR; - if (!_i2c_sem->take(0)) + if (!_dev->get_semaphore()->take(0)) return; - hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, 1, &data); - - _i2c_sem->give(); + _dev->transfer(&data, sizeof(data), nullptr, 0); + _dev->get_semaphore()->give(); } void RCOutput_Bebop::_play_sound(uint8_t sound) { - if (!_i2c_sem->take(0)) + if (!_dev->get_semaphore()->take(0)) return; - hal.i2c1->writeRegister(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_PLAY_SOUND, sound); - - _i2c_sem->give(); + _dev->write_register(BEBOP_BLDC_PLAY_SOUND, sound); + _dev->get_semaphore()->give(); } uint16_t RCOutput_Bebop::_period_us_to_rpm(uint16_t period_us) @@ -243,12 +237,6 @@ void RCOutput_Bebop::init() pthread_attr_t attr; pthread_condattr_t cond_attr; - _i2c_sem = hal.i2c1->get_semaphore(); - if (_i2c_sem == NULL) { - AP_HAL::panic("RCOutput_Bebop: can't get i2c sem"); - return; /* never reached */ - } - /* Initialize thread, cond, and mutex */ ret = pthread_mutex_init(&_mutex, NULL); if (ret != 0) { diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.h b/libraries/AP_HAL_Linux/RCOutput_Bebop.h index 529a1463f9..feabb94dd5 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.h +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.h @@ -1,6 +1,7 @@ #pragma once #include "AP_HAL_Linux.h" +#include enum bebop_bldc_motor { BEBOP_BLDC_MOTOR_1 = 0, @@ -59,7 +60,7 @@ struct bldc_info { class Linux::RCOutput_Bebop : public AP_HAL::RCOutput { public: - RCOutput_Bebop(); + RCOutput_Bebop(AP_HAL::OwnPtr dev); static RCOutput_Bebop *from(AP_HAL::RCOutput *rcout) { return static_cast(rcout); @@ -79,7 +80,7 @@ public: int read_obs_data(BebopBLDC_ObsData &data); private: - AP_HAL::Semaphore *_i2c_sem; + AP_HAL::OwnPtr _dev; uint16_t _request_period_us[BEBOP_BLDC_MOTORS_NUM]; uint16_t _period_us[BEBOP_BLDC_MOTORS_NUM]; uint16_t _rpm[BEBOP_BLDC_MOTORS_NUM];