HAL_Linux: fixed motor output for Disco

libraries/AP_HAL_Linux/RCOutput_Bebop.cpp

@@ -52,9 +52,11 @@ struct PACKED bldc_info {
 };
 
 #define BEBOP_BLDC_TOGGLE_GPIO 0x4d
-#define BEBOP_BLDC_GPIO_RESET   (1 << 0)
+#define BEBOP_BLDC_GPIO_0       (1 << 0)
-#define BEBOP_BLDC_GPIO_RED     (1 << 1)
+#define BEBOP_BLDC_GPIO_1       (1 << 1)
-#define BEBOP_BLDC_GPIO_GREEN   (1 << 2)
+#define BEBOP_BLDC_GPIO_2       (1 << 2)
+#define BEBOP_BLDC_GPIO_3       (1 << 3)
+#define BEBOP_BLDC_GPIO_POWER   (1 << 4)
 
 #define BEBOP_BLDC_STOP_PROP 0x60
 
@@ -66,10 +68,11 @@ struct PACKED bldc_info {
 
 #define BEBOP_BLDC_MIN_PERIOD_US 1100
 #define BEBOP_BLDC_MAX_PERIOD_US 1900
-#define BEBOP_BLDC_MIN_RPM 3000
+#define BEBOP_BLDC_MIN_RPM 1000
 /* the max rpm speed is different on Bebop 2 */
 #define BEBOP_BLDC_MAX_RPM_1 11000
 #define BEBOP_BLDC_MAX_RPM_2 12200
+#define BEBOP_BLDC_MAX_RPM_DISCO 12500
 
 /* Priority of the thread controlling the BLDC via i2c
  * set to 14, which is the same as the UART
@@ -83,6 +86,14 @@ enum {
     BEBOP_BLDC_STOPPED,
 };
 
+/* values of bottom nibble of the obs data status byte */
+enum BLDC_STATUS {
+    BEBOP_BLDC_STATUS_STOPPED=1,
+    BEBOP_BLDC_STATUS_RAMPUP=2,
+    BEBOP_BLDC_STATUS_RUNNING=4,
+    BEBOP_BLDC_STATUS_RAMPDOWN=5
+};
+
 using namespace Linux;
 
 static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
@@ -125,7 +136,12 @@ void RCOutput_Bebop::_start_prop()
 
 void RCOutput_Bebop::_set_ref_speed(uint16_t rpm[BEBOP_BLDC_MOTORS_NUM])
 {
-    struct bldc_ref_speed_data data;
+    struct PACKED bldc_ref_speed_data {
+        uint8_t     cmd;
+        uint16_t    rpm[BEBOP_BLDC_MOTORS_NUM];
+        uint8_t     enable_security;
+        uint8_t     checksum;
+    } data {};
     int i;
 
     data.cmd = BEBOP_BLDC_SETREFSPEED;
@@ -164,8 +180,25 @@ bool RCOutput_Bebop::_get_info(struct bldc_info *info)
 
 int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs)
 {
-    struct bldc_obs_data data;
+    /*
-    int i;
+      the structure returned is different on the Disco from the Bebop
+     */
+    struct PACKED bldc_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;
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
+        /*
+          bit 0 indicates an overcurrent on the RC receiver port when high
+          bits #1-#6 indicate an overcurrent on the #1-#6 PPM servos
+         */
+        uint8_t     overcurrent;
+#endif
+        uint8_t     checksum;
+    } data;
 
     memset(&data, 0, sizeof(data));
     if (!_dev->get_semaphore()->take(0)) {
@@ -175,12 +208,14 @@ int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs)
     _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)) {
+    if (data.checksum != _checksum((uint8_t*)&data, sizeof(data)-1)) {
         return -EBUSY;
     }
 
+    memset(&obs, 0, sizeof(obs));
+    
     /* fill obs class */
-    for (i = 0; i < BEBOP_BLDC_MOTORS_NUM; i++) {
+    for (uint8_t i = 0; i < _n_motors; i++) {
         /* extract 'rpm saturation bit' */
         obs.rpm_saturated[i] = (data.rpm[i] & (1 << 7)) ? 1 : 0;
         /* clear 'rpm saturation bit' */
@@ -189,8 +224,27 @@ int RCOutput_Bebop::read_obs_data(BebopBLDC_ObsData &obs)
         if (obs.rpm[i] == 0) {
             obs.rpm_saturated[i] = 0;
         }
+#if 0
+        printf("rpm %u %u %u %u status 0x%02x temp %u\n",
+               obs.rpm[i], _rpm[0], _period_us[0], _period_us_to_rpm(_period_us[0]),
+               (unsigned)data.status,
+               (unsigned)data.temp);
+#endif
     }
 
+    // sync our state from status. This makes us more robust to i2c errors
+    enum BLDC_STATUS bldc_status = (enum BLDC_STATUS)(data.status & 0x0F);
+    switch (bldc_status) {
+    case BEBOP_BLDC_STATUS_STOPPED:
+    case BEBOP_BLDC_STATUS_RAMPDOWN:
+        _state = BEBOP_BLDC_STOPPED;
+        break;
+    case BEBOP_BLDC_STATUS_RAMPUP:
+    case BEBOP_BLDC_STATUS_RUNNING:
+        _state = BEBOP_BLDC_STARTED;
+        break;
+    }
+    
     obs.batt_mv = be16toh(data.batt_mv);
     obs.status = data.status;
     obs.error = data.error;
@@ -213,7 +267,6 @@ void RCOutput_Bebop::_toggle_gpio(uint8_t mask)
 void RCOutput_Bebop::_stop_prop()
 {
     uint8_t data = BEBOP_BLDC_STOP_PROP;
-    _state = BEBOP_BLDC_STOPPED;
 
     if (!_dev->get_semaphore()->take(0)) {
         return;
@@ -247,8 +300,8 @@ void RCOutput_Bebop::_play_sound(uint8_t sound)
 
 uint16_t RCOutput_Bebop::_period_us_to_rpm(uint16_t period_us)
 {
+    period_us = constrain_int16(period_us, _min_pwm, _max_pwm);
     float period_us_fl = period_us;
-
     float rpm_fl = (period_us_fl - _min_pwm)/(_max_pwm - _min_pwm) *
                     (_max_rpm - BEBOP_BLDC_MIN_RPM) + BEBOP_BLDC_MIN_RPM;
 
@@ -299,6 +352,9 @@ void RCOutput_Bebop::init()
     /* Set an initial dummy frequency */
     _frequency = 50;
 
+    // enable servo power (also receiver power)
+    _toggle_gpio(BEBOP_BLDC_GPIO_2 | BEBOP_BLDC_GPIO_POWER);
+    
 exit:
     pthread_mutex_unlock(&_mutex);
     return;
@@ -348,7 +404,7 @@ void RCOutput_Bebop::push()
     memcpy(_period_us, _request_period_us, sizeof(_period_us));
     pthread_cond_signal(&_cond);
     pthread_mutex_unlock(&_mutex);
-    memset(_request_period_us, 0 ,sizeof(_request_period_us));
+    memset(_request_period_us, 0, sizeof(_request_period_us));
 }
 
 uint16_t RCOutput_Bebop::read(uint8_t ch)
@@ -388,9 +444,7 @@ void RCOutput_Bebop::_run_rcout()
     int ret;
     struct timespec ts;
     struct bldc_info info;
-    uint8_t bebop_bldc_channels[BEBOP_BLDC_MOTORS_NUM];
+    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;
     int hw_version;
 
     memset(current_period_us, 0, sizeof(current_period_us));
@@ -399,11 +453,17 @@ void RCOutput_Bebop::_run_rcout()
         AP_HAL::panic("failed to get BLDC info");
     }
 
+    // remember _n_motors for read_obs_data()
+    _n_motors = info.n_motors;
+
+#if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_DISCO
+    uint8_t bebop_bldc_right_front, bebop_bldc_left_front,
+            bebop_bldc_left_back, bebop_bldc_right_back;
     /* 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) {
+    if (info.version_maj == 1 || info.version_maj == 5) {
         bebop_bldc_right_front = BEBOP_BLDC_MOTOR_1;
         bebop_bldc_left_front  = BEBOP_BLDC_MOTOR_2;
         bebop_bldc_left_back   = BEBOP_BLDC_MOTOR_3;
@@ -419,25 +479,31 @@ void RCOutput_Bebop::_run_rcout()
     bebop_bldc_channels[1] = bebop_bldc_left_back;
     bebop_bldc_channels[2] = bebop_bldc_left_front;
     bebop_bldc_channels[3] = bebop_bldc_right_back;
-
+#endif
+    
     hw_version = Util::from(hal.util)->get_hw_arm32();
     if (hw_version == UTIL_HARDWARE_BEBOP) {
         _max_rpm = BEBOP_BLDC_MAX_RPM_1;
     } else if (hw_version == UTIL_HARDWARE_BEBOP2) {
         _max_rpm = BEBOP_BLDC_MAX_RPM_2;
     } else if (hw_version == UTIL_HARDWARE_DISCO) {
-        _max_rpm = BEBOP_BLDC_MAX_RPM_2;
+        _max_rpm = BEBOP_BLDC_MAX_RPM_DISCO;
     } else if (hw_version < 0) {
         AP_HAL::panic("failed to get hw version %s", strerror(hw_version));
     } else {
         AP_HAL::panic("unsupported hw version %d", hw_version);
     }
+    printf("Bebop: vers %u/%u type %u nmotors %u n_flights %u last_flight_time %u total_flight_time %u maxrpm %u\n",
+           (unsigned)info.version_maj, (unsigned)info.version_min, (unsigned)info.type,
+           (unsigned)info.n_motors, (unsigned)be16toh(info.n_flights),
+           (unsigned)be16toh(info.last_flight_time), (unsigned)be32toh(info.total_flight_time),
+           (unsigned)_max_rpm);
 
     while (true) {
         pthread_mutex_lock(&_mutex);
         ret = clock_gettime(CLOCK_MONOTONIC, &ts);
         if (ret != 0) {
-            hal.console->println("RCOutput_Bebop: bad checksum in obs data");
+            continue;
         }
 
         if (ts.tv_nsec > (1000000000 - BEBOP_BLDC_TIMEOUT_NS)) {
@@ -457,14 +523,14 @@ void RCOutput_Bebop::_run_rcout()
 
         /* start propellers if the speed of the 4 motors is >= min speed
          * min speed set to min_pwm + 50*/
-        for (i = 0; i < BEBOP_BLDC_MOTORS_NUM; i++) {
+        for (i = 0; i < _n_motors; i++) {
-            if (current_period_us[i] <= _min_pwm + 50) {
+            if (current_period_us[i] <= _min_pwm + 50)
                 break;
             }
             _rpm[bebop_bldc_channels[i]] = _period_us_to_rpm(current_period_us[i]);
         }
 
-        if (i < BEBOP_BLDC_MOTORS_NUM) {
+        if (i < _n_motors) {
             /* one motor pwm value is at minimum (or under)
              * if the motors are started, stop them*/
             if (_state == BEBOP_BLDC_STARTED) {
@@ -477,8 +543,8 @@ void RCOutput_Bebop::_run_rcout()
             if (_state == BEBOP_BLDC_STOPPED) {
                 _start_prop();
             }
+            _set_ref_speed(_rpm);
         }
-        _set_ref_speed(_rpm);
     }
 }
 

libraries/AP_HAL_Linux/RCOutput_Bebop.h

@@ -5,9 +5,11 @@
 
 enum bebop_bldc_motor {
     BEBOP_BLDC_MOTOR_1 = 0,
+#if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_DISCO
     BEBOP_BLDC_MOTOR_2,
     BEBOP_BLDC_MOTOR_3,
     BEBOP_BLDC_MOTOR_4,
+#endif
     BEBOP_BLDC_MOTORS_NUM,
 };
 
@@ -78,6 +80,7 @@ private:
     uint16_t _frequency;
     uint16_t _min_pwm;
     uint16_t _max_pwm;
+    uint8_t _n_motors=4;
     uint8_t  _state;
     bool     _corking = false;
     uint16_t _max_rpm;

libraries/AP_HAL_Linux/RCOutput_Disco.cpp

@@ -25,6 +25,7 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
+#include <stdio.h>
 
 namespace Linux {
 
@@ -32,6 +33,7 @@ void RCOutput_Disco::init()
 {
     sysfs_out.init();
     bebop_out.init();
+    printf("RCOutput_Disco: initialised\n");
 }
 
 void RCOutput_Disco::set_freq(uint32_t chmask, uint16_t freq_hz)

libraries/AP_HAL_Linux/RCOutput_Disco.h

@@ -9,9 +9,12 @@ public:
     RCOutput_Disco(void) {}
     ~RCOutput_Disco() {}
 
-    static RCOutput_Disco *from(AP_HAL::RCOutput *rcoutput)
+    static RCOutput_Bebop *from(AP_HAL::RCOutput *rcoutput)
     {
-        return static_cast<RCOutput_Disco *>(rcoutput);
+        // this is used by AP_BattMonitor_Bebop to get obs data. We
+        // need to return the Bebop output, not ourselves
+        RCOutput_Disco *d = static_cast<RCOutput_Disco *>(rcoutput);
+        return static_cast<RCOutput_Bebop *>(&d->bebop_out);
     }
 
     void init() override;
@@ -27,8 +30,8 @@ public:
 
 private:
     // Disco RC output combines methods from Sysfs and Bebop
-    RCOutput_Sysfs sysfs_out{0, 1, 6};
     RCOutput_Bebop bebop_out;
+    RCOutput_Sysfs sysfs_out{0, 1, 6};
 
     /*
       use a table to provide the mapping to channel numbers in each