Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: RCOutput_Bebop: use I2CDevice interface

This commit is contained in:
Luiz Ywata 2016-04-14 17:40:45 -03:00 committed by Lucas De Marchi
parent a34a5c1aa3
commit 7fb5db8077
4 changed files with 34 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -231,6 +231,7 @@
#define HAL_FLOW_PX4_BOTTOM_FLOW_FEATURE_THRESHOLD 30 #define HAL_FLOW_PX4_BOTTOM_FLOW_FEATURE_THRESHOLD 30
#define HAL_FLOW_PX4_BOTTOM_FLOW_VALUE_THRESHOLD 5000 #define HAL_FLOW_PX4_BOTTOM_FLOW_VALUE_THRESHOLD 5000
#define HAL_PARAM_DEFAULTS_PATH "/etc/arducopter/bebop.parm" #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 /* focal length 3.6 um, 2x binning in each direction
* 240x240 crop rescaled to 64x64 */ * 240x240 crop rescaled to 64x64 */
#define HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX (2.5 / (3.6 * 2.0 * 240 / 64)) #define HAL_FLOW_PX4_FOCAL_LENGTH_MILLIPX (2.5 / (3.6 * 2.0 * 240 / 64))

2
libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
View File

@ -170,7 +170,7 @@ static RCOutput_Raspilot rcoutDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ
static RCOutput_ZYNQ rcoutDriver; static RCOutput_ZYNQ rcoutDriver;
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP #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 #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(PCA9685_PRIMARY_ADDRESS, false, 0, MINNOW_GPIO_S5_1);
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT

70
libraries/AP_HAL_Linux/RCOutput_Bebop.cpp
View File

@ -14,9 +14,10 @@
#include "RCOutput_Bebop.h" #include "RCOutput_Bebop.h"
#include "Util.h" #include "Util.h"
#include <utility>
/* 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_STARTPROP 0x40
#define BEBOP_BLDC_SETREFSPEED 0x02 #define BEBOP_BLDC_SETREFSPEED 0x02
@ -74,8 +75,8 @@ using namespace Linux;
static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
RCOutput_Bebop::RCOutput_Bebop(): RCOutput_Bebop::RCOutput_Bebop(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev):
_i2c_sem(NULL), _dev(std::move(dev)),
_min_pwm(BEBOP_BLDC_MIN_PERIOD_US), _min_pwm(BEBOP_BLDC_MIN_PERIOD_US),
_max_pwm(BEBOP_BLDC_MAX_PERIOD_US), _max_pwm(BEBOP_BLDC_MAX_PERIOD_US),
_state(BEBOP_BLDC_STOPPED) _state(BEBOP_BLDC_STOPPED)
@ -100,12 +101,12 @@ void RCOutput_Bebop::_start_prop()
{ {
uint8_t data = BEBOP_BLDC_STARTPROP; uint8_t data = BEBOP_BLDC_STARTPROP;
if (!_i2c_sem->take(0)) if (!_dev->get_semaphore()->take(0))
return; 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; _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.enable_security = 0;
data.checksum = _checksum((uint8_t *) &data, sizeof(data) - 1); data.checksum = _checksum((uint8_t *) &data, sizeof(data) - 1);
if (!_i2c_sem->take(0)) if (!_dev->get_semaphore()->take(0))
return; 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) bool RCOutput_Bebop::_get_info(struct bldc_info *info)
@ -136,12 +137,11 @@ bool RCOutput_Bebop::_get_info(struct bldc_info *info)
return false; return false;
} }
memset(info, 0, sizeof(struct bldc_info)); memset(info, 0, sizeof(struct bldc_info));
if (!_i2c_sem->take(0)) { if (!_dev->get_semaphore()->take(0)) {
return false; return false;
} }
hal.i2c1->readRegisters(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_GET_INFO, _dev->read_registers(BEBOP_BLDC_GET_INFO, (uint8_t*)info, sizeof(*info));
sizeof(struct bldc_info), (uint8_t *)info); _dev->get_semaphore()->give();
_i2c_sem->give();
return true; return true;
} }
@ -151,13 +151,11 @@ int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs)
int i; int i;
memset(&data, 0, sizeof(data)); memset(&data, 0, sizeof(data));
if (!_i2c_sem->take(0)) if (!_dev->get_semaphore()->take(0))
return -EBUSY; return -EBUSY;
hal.i2c1->readRegisters(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_GETOBSDATA, _dev->read_registers(BEBOP_BLDC_GETOBSDATA, (uint8_t *)&data, sizeof(data));
sizeof(data), (uint8_t *)&data); _dev->get_semaphore()->give();
_i2c_sem->give();
if (data.checksum != _checksum((uint8_t *)&data, sizeof(data) - 1)) if (data.checksum != _checksum((uint8_t *)&data, sizeof(data) - 1))
hal.console->printf("RCOutput_Bebop: bad checksum in obs data"); 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) void RCOutput_Bebop::_toggle_gpio(uint8_t mask)
{ {
if (!_i2c_sem->take(0)) if (!_dev->get_semaphore()->take(0))
return; return;
hal.i2c1->writeRegister(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_TOGGLE_GPIO, mask); _dev->write_register(BEBOP_BLDC_TOGGLE_GPIO, mask);
_dev->get_semaphore()->give();
_i2c_sem->give();
} }
void RCOutput_Bebop::_stop_prop() void RCOutput_Bebop::_stop_prop()
@ -196,34 +193,31 @@ void RCOutput_Bebop::_stop_prop()
uint8_t data = BEBOP_BLDC_STOP_PROP; uint8_t data = BEBOP_BLDC_STOP_PROP;
_state = BEBOP_BLDC_STOPPED; _state = BEBOP_BLDC_STOPPED;
if (!_i2c_sem->take(0)) if (!_dev->get_semaphore()->take(0))
return; return;
hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, 1, &data); _dev->transfer(&data, sizeof(data), nullptr, 0);
_dev->get_semaphore()->give();
_i2c_sem->give();
} }
void RCOutput_Bebop::_clear_error() void RCOutput_Bebop::_clear_error()
{ {
uint8_t data = BEBOP_BLDC_CLEAR_ERROR; uint8_t data = BEBOP_BLDC_CLEAR_ERROR;
if (!_i2c_sem->take(0)) if (!_dev->get_semaphore()->take(0))
return; return;
hal.i2c1->write(BEBOP_BLDC_I2C_ADDR, 1, &data); _dev->transfer(&data, sizeof(data), nullptr, 0);
_dev->get_semaphore()->give();
_i2c_sem->give();
} }
void RCOutput_Bebop::_play_sound(uint8_t sound) void RCOutput_Bebop::_play_sound(uint8_t sound)
{ {
if (!_i2c_sem->take(0)) if (!_dev->get_semaphore()->take(0))
return; return;
hal.i2c1->writeRegister(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_PLAY_SOUND, sound); _dev->write_register(BEBOP_BLDC_PLAY_SOUND, sound);
_dev->get_semaphore()->give();
_i2c_sem->give();
} }
uint16_t RCOutput_Bebop::_period_us_to_rpm(uint16_t period_us) 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_attr_t attr;
pthread_condattr_t cond_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 */ /* Initialize thread, cond, and mutex */
ret = pthread_mutex_init(&_mutex, NULL); ret = pthread_mutex_init(&_mutex, NULL);
if (ret != 0) { if (ret != 0) {

5
libraries/AP_HAL_Linux/RCOutput_Bebop.h
View File

@ -1,6 +1,7 @@
#pragma once #pragma once
#include "AP_HAL_Linux.h" #include "AP_HAL_Linux.h"
#include <AP_HAL/I2CDevice.h>
enum bebop_bldc_motor { enum bebop_bldc_motor {
BEBOP_BLDC_MOTOR_1 = 0, BEBOP_BLDC_MOTOR_1 = 0,
@ -59,7 +60,7 @@ struct bldc_info {
class Linux::RCOutput_Bebop : public AP_HAL::RCOutput { class Linux::RCOutput_Bebop : public AP_HAL::RCOutput {
public: public:
RCOutput_Bebop(); RCOutput_Bebop(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
static RCOutput_Bebop *from(AP_HAL::RCOutput *rcout) { static RCOutput_Bebop *from(AP_HAL::RCOutput *rcout) {
return static_cast<RCOutput_Bebop*>(rcout); return static_cast<RCOutput_Bebop*>(rcout);
@ -79,7 +80,7 @@ public:
int read_obs_data(BebopBLDC_ObsData &data); int read_obs_data(BebopBLDC_ObsData &data);
private: private:
AP_HAL::Semaphore *_i2c_sem; AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint16_t _request_period_us[BEBOP_BLDC_MOTORS_NUM]; uint16_t _request_period_us[BEBOP_BLDC_MOTORS_NUM];
uint16_t _period_us[BEBOP_BLDC_MOTORS_NUM]; uint16_t _period_us[BEBOP_BLDC_MOTORS_NUM];
uint16_t _rpm[BEBOP_BLDC_MOTORS_NUM]; uint16_t _rpm[BEBOP_BLDC_MOTORS_NUM];