Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 14:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: add read_obs_data to RCOutput_Bebop

Browse Source
This commit is contained in:
Jean-Baptiste Dubois 2015-07-23 18:18:30 +09:00 committed by Randy Mackay
parent 35ae562dbe
commit f6aba6c952
2 changed files with 64 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
libraries/AP_HAL_Linux/RCOutput_Bebop.cpp
View File

@ -14,16 +14,15 @@
/* BEBOP BLDC motor controller address and registers description */ /* BEBOP BLDC motor controller address and registers description */
#define BEBOP_BLDC_I2C_ADDR 0x08 #define BEBOP_BLDC_I2C_ADDR 0x08
#define BEBOP_BLDC_STARTPROP 0x40 #define BEBOP_BLDC_STARTPROP 0x40
#define BEBOP_BLDC_CLOCKWISE 1 #define BEBOP_BLDC_CLOCKWISE 1
#define BEBOP_BLDC_COUNTERCLOCKWISE 0 #define BEBOP_BLDC_COUNTERCLOCKWISE 0
static const uint8_t bebop_motors_bitmask = (BEBOP_BLDC_COUNTERCLOCKWISE << static const uint8_t bebop_motors_bitmask = (BEBOP_BLDC_COUNTERCLOCKWISE <<
(BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_LEFT_BACK)) | (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_LEFT_BACK)) |
(BEBOP_BLDC_CLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_RIGHT_BACK))| (BEBOP_BLDC_CLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_RIGHT_BACK))|
(BEBOP_BLDC_COUNTERCLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_RIGHT_FRONT))| (BEBOP_BLDC_COUNTERCLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_RIGHT_FRONT))|
(BEBOP_BLDC_CLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_LEFT_FRONT)); (BEBOP_BLDC_CLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_LEFT_FRONT));
#define BEBOP_BLDC_SETREFSPEED 0x02 #define BEBOP_BLDC_SETREFSPEED 0x02
@ -45,28 +44,10 @@ struct bldc_obs_data {
uint8_t checksum; uint8_t checksum;
}__attribute__((packed)); }__attribute__((packed));
/* description of the bldc status */
#define BEBOP_BLDC_STATUS_INIT 0
#define BEBOP_BLDC_STATUS_IDLE 1
#define BEBOP_BLDC_STATUS_RAMPING 2
#define BEBOP_BLDC_STATUS_SPINNING_1 3
#define BEBOP_BLDC_STATUS_SPINNING_2 4
#define BEBOP_BLDC_STATUS_STOPPING 5
#define BEBOP_BLDC_STATUS_CRITICAL 6
/* description of the bldc errno */
#define BEBOP_BLDC_ERRNO_EEPROM 1
#define BEBOP_BLDC_ERRNO_MOTOR_STALLED 2
#define BEBOP_BLDC_ERRNO_PROP_SECU 3
#define BEBOP_BLDC_ERRNO_COM_LOST 4
#define BEBOP_BLDC_ERRNO_BATT_LEVEL 9
#define BEBOP_BLDC_ERRNO_LIPO 10
#define BEBOP_BLDC_ERRNO_MOTOR_HW 11
#define BEBOP_BLDC_TOGGLE_GPIO 0x4d #define BEBOP_BLDC_TOGGLE_GPIO 0x4d
#define BEBOP_BLDC_GPIO_RESET (1 << 0) #define BEBOP_BLDC_GPIO_RESET (1 << 0)
#define BEBOP_BLDC_GPIO_RED (1 << 1) #define BEBOP_BLDC_GPIO_RED (1 << 1)
#define BEBOP_BLDC_GPIO_GREEN (1 << 2) #define BEBOP_BLDC_GPIO_GREEN (1 << 2)
#define BEBOP_BLDC_STOP_PROP 0x60 #define BEBOP_BLDC_STOP_PROP 0x60
@ -166,48 +147,40 @@ void LinuxRCOutput_Bebop::_set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM])
_i2c_sem->give(); _i2c_sem->give();
} }
void LinuxRCOutput_Bebop::_get_obs_data(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM], int LinuxRCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs)
uint16_t *batt_mv,
uint8_t *status,
uint8_t *error,
uint8_t *motors_err,
uint8_t *temp)
{ {
struct bldc_obs_data data; struct bldc_obs_data data;
int i; int i;
memset(&data, 0, sizeof(data)); memset(&data, 0, sizeof(data));
if (!_i2c_sem->take(0)) if (!_i2c_sem->take(0))
return; return -EBUSY;
hal.i2c1->readRegisters(BEBOP_BLDC_I2C_ADDR, hal.i2c1->readRegisters(BEBOP_BLDC_I2C_ADDR, BEBOP_BLDC_GETOBSDATA,
BEBOP_BLDC_GETOBSDATA, sizeof(data), (uint8_t *)&data);
sizeof(data),
(uint8_t *)&data);
if (data.checksum != _checksum((uint8_t *)&data, sizeof(data))) _i2c_sem->give();
hal.console->println_P("RCOutput_Bebop: bad checksum in obs data");
if (rpm != NULL) { if (data.checksum != _checksum((uint8_t *)&data, sizeof(data) - 1))
for(i=0; i<BEBOP_BLDC_MOTORS_NUM; i++) hal.console->printf("RCOutput_Bebop: bad checksum in obs data");
rpm[i] = be16toh(data.rpm[i]);
/* fill obs class */
for (i = 0; i < BEBOP_BLDC_MOTORS_NUM; i++) {
/* extract 'rpm saturation bit' */
obs.rpm_saturated[i] = (data.rpm[i] & (1 << 7)) ? 1 : 0;
/* clear 'rpm saturation bit' */
data.rpm[i] &= (uint16_t)(~(1 << 7));
obs.rpm[i] = be16toh(data.rpm[i]);
if (obs.rpm[i] == 0)
obs.rpm_saturated[i] = 0;
} }
if (batt_mv != NULL) obs.batt_mv = be16toh(data.batt_mv);
*batt_mv = be16toh(data.batt_mv); obs.status = data.status;
obs.error = data.error;
if (status != NULL) obs.motors_err = data.motors_err;
*status = data.status; obs.temperature = data.temp;
return 0;
if (error != NULL)
*error = data.error;
if (motors_err != NULL)
*motors_err = data.motors_err;
if (temp != NULL)
*temp = data.temp;
} }
void LinuxRCOutput_Bebop::_toggle_gpio(uint8_t mask) void LinuxRCOutput_Bebop::_toggle_gpio(uint8_t mask)

40
libraries/AP_HAL_Linux/RCOutput_Bebop.h
View File

@ -18,8 +18,37 @@ enum bebop_bldc_sound {
BEBOP_BLDC_SOUND_BEBOP, BEBOP_BLDC_SOUND_BEBOP,
}; };
/* description of the bldc status */
#define BEBOP_BLDC_STATUS_INIT 0
#define BEBOP_BLDC_STATUS_IDLE 1
#define BEBOP_BLDC_STATUS_RAMPING 2
#define BEBOP_BLDC_STATUS_SPINNING_1 3
#define BEBOP_BLDC_STATUS_SPINNING_2 4
#define BEBOP_BLDC_STATUS_STOPPING 5
#define BEBOP_BLDC_STATUS_CRITICAL 6
/* description of the bldc errno */
#define BEBOP_BLDC_ERRNO_EEPROM 1
#define BEBOP_BLDC_ERRNO_MOTOR_STALLED 2
#define BEBOP_BLDC_ERRNO_PROP_SECU 3
#define BEBOP_BLDC_ERRNO_COM_LOST 4
#define BEBOP_BLDC_ERRNO_BATT_LEVEL 9
#define BEBOP_BLDC_ERRNO_LIPO 10
#define BEBOP_BLDC_ERRNO_MOTOR_HW 11
class BebopBLDC_ObsData {
public:
uint16_t rpm[BEBOP_BLDC_MOTORS_NUM];
uint8_t rpm_saturated[BEBOP_BLDC_MOTORS_NUM];
uint16_t batt_mv;
uint8_t status;
uint8_t error;
uint8_t motors_err;
uint8_t temperature;
};
class Linux::LinuxRCOutput_Bebop : public AP_HAL::RCOutput { class Linux::LinuxRCOutput_Bebop : public AP_HAL::RCOutput {
public: public:
LinuxRCOutput_Bebop(); LinuxRCOutput_Bebop();
void init(void* dummy); void init(void* dummy);
void set_freq(uint32_t chmask, uint16_t freq_hz); void set_freq(uint32_t chmask, uint16_t freq_hz);
@ -31,6 +60,7 @@ class Linux::LinuxRCOutput_Bebop : public AP_HAL::RCOutput {
uint16_t read(uint8_t ch); uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len); void read(uint16_t* period_us, uint8_t len);
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm); void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm);
int read_obs_data(BebopBLDC_ObsData &data);
private: private:
AP_HAL::Semaphore *_i2c_sem; AP_HAL::Semaphore *_i2c_sem;
@ -44,14 +74,8 @@ private:
uint8_t _checksum(uint8_t *data, unsigned int len); uint8_t _checksum(uint8_t *data, unsigned int len);
void _start_prop(); void _start_prop();
void _set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM]);
void _get_obs_data(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM],
uint16_t *batt_mv,
uint8_t *status,
uint8_t *error,
uint8_t *motors_err,
uint8_t *temp);
void _toggle_gpio(uint8_t mask); void _toggle_gpio(uint8_t mask);
void _set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM]);
void _stop_prop(); void _stop_prop();
void _clear_error(); void _clear_error();
void _play_sound(uint8_t sound); void _play_sound(uint8_t sound);