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_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))

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
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

70
libraries/AP_HAL_Linux/RCOutput_Bebop.cpp
View File

@ -14,9 +14,10 @@
#include "RCOutput_Bebop.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_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<AP_HAL::I2CDevice> 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) {

5
libraries/AP_HAL_Linux/RCOutput_Bebop.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "AP_HAL_Linux.h"
#include <AP_HAL/I2CDevice.h>
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<AP_HAL::I2CDevice> dev);
static RCOutput_Bebop *from(AP_HAL::RCOutput *rcout) {
return static_cast<RCOutput_Bebop*>(rcout);
@ -79,7 +80,7 @@ public:
int read_obs_data(BebopBLDC_ObsData &data);
private:
AP_HAL::Semaphore *_i2c_sem;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _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];