RCOutput_Bebop : Update motor order

Newer esc firmware versions on bebop 1 and all the versions on bebop 2
have a different order for the motors in the i2c frame sent to the
esc contoller. This commit adds support for both versions by reading
the firmware version of the esc, using GET_INFO frame

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

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_MOTOR_1 = 0,
-    BEBOP_BLDC_LEFT_FRONT,
+    BEBOP_BLDC_MOTOR_2,
-    BEBOP_BLDC_LEFT_BACK,
+    BEBOP_BLDC_MOTOR_3,
-    BEBOP_BLDC_RIGHT_BACK,
+    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);