diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp index 021b74f502..efcacc23cc 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp @@ -15,15 +15,6 @@ /* BEBOP BLDC motor controller address and registers description */ #define BEBOP_BLDC_I2C_ADDR 0x08 #define BEBOP_BLDC_STARTPROP 0x40 -#define BEBOP_BLDC_CLOCKWISE 1 -#define BEBOP_BLDC_COUNTERCLOCKWISE 0 - -static const uint8_t bebop_motors_bitmask = (BEBOP_BLDC_COUNTERCLOCKWISE << - (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_LEFT_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_CLOCKWISE << (BEBOP_BLDC_MOTORS_NUM - 1 - BEBOP_BLDC_LEFT_FRONT)); - #define BEBOP_BLDC_SETREFSPEED 0x02 struct bldc_ref_speed_data { @@ -57,19 +48,6 @@ struct bldc_obs_data { #define BEBOP_BLDC_GET_INFO 0xA0 -/* Bebop is a Quad X so the channels are : - * 1 = Front Right - * 2 = Back Left - * 3 = Front Left - * 4 = Back Right - * - * but the channels start at 0 so it is channel num - 1 -*/ -static const uint8_t bebop_bldc_motors[BEBOP_BLDC_MOTORS_NUM] = { BEBOP_BLDC_RIGHT_FRONT, - BEBOP_BLDC_LEFT_BACK, - BEBOP_BLDC_LEFT_FRONT, - BEBOP_BLDC_RIGHT_BACK }; - #define BEBOP_BLDC_MIN_PERIOD_US 1100 #define BEBOP_BLDC_MAX_PERIOD_US 1900 #define BEBOP_BLDC_MIN_RPM 3000 @@ -147,6 +125,21 @@ void RCOutput_Bebop::_set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM]) _i2c_sem->give(); } +bool RCOutput_Bebop::_get_info(struct bldc_info *info) +{ + if (info == NULL) { + return false; + } + memset(info, 0, sizeof(struct bldc_info)); + if (!_i2c_sem->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(); + return true; +} + int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs) { struct bldc_obs_data data; @@ -373,9 +366,38 @@ void RCOutput_Bebop::_run_rcout() uint8_t i; int ret; struct timespec ts; + struct bldc_info info; + uint8_t bebop_bldc_channels[BEBOP_BLDC_MOTORS_NUM]; + uint8_t bebop_bldc_right_front, bebop_bldc_left_front, + bebop_bldc_left_back, bebop_bldc_right_back; memset(current_period_us, 0, sizeof(current_period_us)); + if (!_get_info(&info)) { + AP_HAL::panic("failed to get BLDC info\n"); + } + + /* Set motor order depending on BLDC version.On bebop 1 with version 1 + * keep current order. The order changes from version 2 on bebop 1 and + * remains the same as this for bebop 2 + */ + if (info.version_maj == 1) { + bebop_bldc_right_front = BEBOP_BLDC_MOTOR_1; + bebop_bldc_left_front = BEBOP_BLDC_MOTOR_2; + bebop_bldc_left_back = BEBOP_BLDC_MOTOR_3; + bebop_bldc_right_back = BEBOP_BLDC_MOTOR_4; + } else { + bebop_bldc_right_front = BEBOP_BLDC_MOTOR_2; + bebop_bldc_left_front = BEBOP_BLDC_MOTOR_1; + bebop_bldc_left_back = BEBOP_BLDC_MOTOR_4; + bebop_bldc_right_back = BEBOP_BLDC_MOTOR_3; + } + + bebop_bldc_channels[0] = bebop_bldc_right_front; + bebop_bldc_channels[1] = bebop_bldc_left_back; + bebop_bldc_channels[2] = bebop_bldc_left_front; + bebop_bldc_channels[3] = bebop_bldc_right_back; + while (true) { pthread_mutex_lock(&_mutex); ret = clock_gettime(CLOCK_MONOTONIC, &ts); @@ -402,7 +424,7 @@ void RCOutput_Bebop::_run_rcout() for (i = 0; i < BEBOP_BLDC_MOTORS_NUM; i++) { if (current_period_us[i] <= _min_pwm + 50) break; - _rpm[bebop_bldc_motors[i]] = _period_us_to_rpm(current_period_us[i]); + _rpm[bebop_bldc_channels[i]] = _period_us_to_rpm(current_period_us[i]); } if (i < BEBOP_BLDC_MOTORS_NUM) { diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.h b/libraries/AP_HAL_Linux/RCOutput_Bebop.h index b0e7a2536b..672e2f61eb 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.h +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.h @@ -4,10 +4,10 @@ #include "AP_HAL_Linux.h" enum bebop_bldc_motor { - BEBOP_BLDC_RIGHT_FRONT = 0, - BEBOP_BLDC_LEFT_FRONT, - BEBOP_BLDC_LEFT_BACK, - BEBOP_BLDC_RIGHT_BACK, + BEBOP_BLDC_MOTOR_1 = 0, + BEBOP_BLDC_MOTOR_2, + BEBOP_BLDC_MOTOR_3, + BEBOP_BLDC_MOTOR_4, BEBOP_BLDC_MOTORS_NUM, }; @@ -47,6 +47,17 @@ public: uint8_t temperature; }; +struct bldc_info { + uint8_t version_maj; + uint8_t version_min; + uint8_t type; + uint8_t n_motors; + uint16_t n_flights; + uint16_t last_flight_time; + uint32_t total_flight_time; + uint8_t last_error; +}__attribute__((packed)); + class Linux::RCOutput_Bebop : public AP_HAL::RCOutput { public: RCOutput_Bebop(); @@ -83,6 +94,7 @@ private: void _start_prop(); void _toggle_gpio(uint8_t mask); void _set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM]); + bool _get_info(struct bldc_info *info); void _stop_prop(); void _clear_error(); void _play_sound(uint8_t sound);