The Mother of All BLHeli Passthrough Fixes

libraries/AP_BLHeli/AP_BLHeli.cpp

@@ -42,10 +42,16 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_ESC_Telem/AP_ESC_Telem.h>
 #include <SRV_Channel/SRV_Channel.h>
+#include <AP_BattMonitor/AP_BattMonitor.h>
 
 extern const AP_HAL::HAL& hal;
 
 #define debug(fmt, args ...) do { if (debug_level) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: " fmt, ## args); } } while (0)
+#ifdef BLHELI_DEBUG
+#define debug_console(fmt, args ...) do { hal.console->printf(fmt "\n", ## args); } while (0)
+#else
+#define debug_console(fmt, args ...) do {} while(0)
+#endif
 
 // key for locking UART for exclusive use. This prevents any other writes from corrupting
 // the MSP protocol on hal.console
@@ -168,6 +174,16 @@ AP_BLHeli::AP_BLHeli(void)
     last_control_port = -1;
 }
 
+// map an incoming BLHeli motor request to the appropriate 
+// output channel for use in serial output so that motor numbers
+// are observed
+uint8_t AP_BLHeli::blheli_chan_to_output_chan(uint8_t motor)
+{
+    uint8_t chan = 0;   // 0 means motor 1 and is ok as a fallback
+    SRV_Channels::find_channel(SRV_Channels::get_motor_function(motor), chan);
+    return chan;
+}
+
 /*
   process one byte of serial input for MSP protocol
  */
@@ -390,9 +406,8 @@ void AP_BLHeli::msp_process_command(void)
 
     case MSP_REBOOT:
         debug("MSP: ignoring reboot command, end serial comms");
-        hal.rcout->serial_end();
+        serial_end();
         blheli.connected[blheli.chan] = false;
-        serial_start_ms = 0;
         break;
 
     case MSP_UID:
@@ -457,12 +472,34 @@ void AP_BLHeli::msp_process_command(void)
 
     case MSP_BATTERY_STATE: {
         debug("MSP_BATTERY_STATE");
-        uint8_t buf[8];
-        buf[0] = 4; // cell count
-        putU16(&buf[1], 1500); // mAh
-        buf[3] = 16; // V
-        putU16(&buf[4], 1500); // mAh
-        putU16(&buf[6], 1); // A
+        // ESC configurator seems to care a lot about the battery state,
+        // try and at least provide something believable
+        uint8_t buf[11] {};
+#if AP_BATTERY_ENABLED
+        AP_BattMonitor &battery = AP::battery();
+        float v;
+#if HAL_WITH_ESC_TELEM
+        if (!AP::esc_telem().get_voltage(blheli_chan_to_output_chan(blheli.chan), v)) {
+            v = battery.voltage();
+        }        
+#else
+            v = battery.voltage();
+#endif
+        buf[0] = battery.healthy() ? uint8_t(roundf(v / 3.85)) : 0; // cell count, 0 means no battery
+        putU16(&buf[1], uint16_t(battery.pack_capacity_mah())); // capacity in mAh
+        buf[3] = uint8_t(roundf(v * 10.0)); // legacy V in 0.1V steps
+
+        float cons = 0;
+        UNUSED_RESULT(battery.consumed_mah(cons));
+        putU16(&buf[4], uint16_t(roundf(cons))); // mAh used
+
+        float amps = 0.0;
+        UNUSED_RESULT(battery.current_amps(amps));
+        putU16(&buf[6], uint16_t(roundf(amps * 100.0))); // A in 0.01A steps
+        buf[8] = battery.healthy(); // alerts/state
+        // We are advertising MSP v1.42 which means supporting the new voltage field
+        putU16(&buf[9], uint16_t(roundf(v * 100.0))); // Voltage in 0.01V steps
+#endif
         msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
         break;
     }
@@ -488,7 +525,7 @@ void AP_BLHeli::msp_process_command(void)
         uint8_t buf[16] {};
         for (uint8_t i = 0; i < num_motors; i++) {
             // if we have a mix of reversible and normal report a PWM of zero, this allows BLHeliSuite to conect
-            uint16_t v = mixed_type ? 0 : hal.rcout->read(motor_map[i]);
+            uint16_t v = mixed_type ? 0 : hal.rcout->read(blheli_chan_to_output_chan(i));
             putU16(&buf[2*i], v);
             debug("MOTOR %u val: %u",i,v);
         }
@@ -515,7 +552,7 @@ void AP_BLHeli::msp_process_command(void)
                 debug("MSP_SET_MOTOR %u %u", i, v);
                 // map from a MSP value to a value in the range 1000 to 2000
                 uint16_t pwm = (v < 1000)?0:v;
-                hal.rcout->write(motor_map[i], pwm);
+                hal.rcout->write(blheli_chan_to_output_chan(i), pwm);
             }
             hal.rcout->push();
         } else {
@@ -540,14 +577,13 @@ void AP_BLHeli::msp_process_command(void)
             break;
         default:
             n = 0;
-            hal.rcout->serial_end();
-            serial_start_ms = 0;
+            serial_end();
             break;
         }
         // doing the serial setup here avoids delays when doing it on demand and makes
         // BLHeliSuite considerably more reliable
         EXPECT_DELAY_MS(1000);
-        if (!hal.rcout->serial_setup_output(motor_map[0], 19200, motor_mask)) {
+        if (!hal.rcout->serial_setup_output(blheli_chan_to_output_chan(0), 19200, motor_mask)) {
             msp_send_ack(ACK_D_GENERAL_ERROR);
             break;
         } else {
@@ -622,18 +658,25 @@ bool AP_BLHeli::BL_SendBuf(const uint8_t *buf, uint16_t len)
         return false;
     }
     EXPECT_DELAY_MS(1000);
-    if (!hal.rcout->serial_setup_output(motor_map[blheli.chan], 19200, motor_mask)) {
+    hal.scheduler->delay_microseconds(100);
+    if (!hal.rcout->serial_setup_output(blheli_chan_to_output_chan(blheli.chan), 19200, motor_mask)) {
+        debug_console("serial_setup_output() failed\n");
         blheli.ack = ACK_D_GENERAL_ERROR;
         return false;
     }
+    // ensure that the next write does not go out immediately in case the receiving side is not yet
+    // ready
+    hal.scheduler->delay_microseconds(100);
+
     if (serial_start_ms == 0) {
         serial_start_ms = AP_HAL::millis();
     }
     memcpy(blheli.buf, buf, len);
     uint16_t crc = BL_CRC(buf, len);
-    blheli.buf[len] = crc;
-    blheli.buf[len+1] = crc>>8;
+    blheli.buf[len] = uint8_t(crc & 0xFF);
+    blheli.buf[len+1] = uint8_t(crc>>8);
     if (!hal.rcout->serial_write_bytes(blheli.buf, len+(send_crc?2:0))) {
+        debug_console("serial_write_bytes() failed\n");
         blheli.ack = ACK_D_GENERAL_ERROR;
         return false;
     }
@@ -648,10 +691,11 @@ bool AP_BLHeli::BL_SendBuf(const uint8_t *buf, uint16_t len)
  */
 bool AP_BLHeli::BL_ReadBuf(uint8_t *buf, uint16_t len)
 {
-    bool check_crc = isMcuConnected() && len > 0;
+    bool check_crc = isMcuConnected();
     uint16_t req_bytes = len+(check_crc?3:1);
     EXPECT_DELAY_MS(1000);
-    uint16_t n = hal.rcout->serial_read_bytes(blheli.buf, req_bytes);
+    // byte time is 520us so 1000 per byte should be more than enough
+    uint16_t n = hal.rcout->serial_read_bytes(blheli.buf, req_bytes, req_bytes * 1000);
     debug("BL_ReadBuf %u -> %u", len, n);
     if (req_bytes != n) {
         debug("short read");
@@ -687,12 +731,9 @@ bool AP_BLHeli::BL_ReadBuf(uint8_t *buf, uint16_t len)
 uint8_t AP_BLHeli::BL_GetACK(uint16_t timeout_ms)
 {
     uint8_t ack;
-    uint32_t start_ms = AP_HAL::millis();
-    EXPECT_DELAY_MS(1000);
-    while (AP_HAL::millis() - start_ms < timeout_ms) {
-        if (hal.rcout->serial_read_bytes(&ack, 1) == 1) {
-            return ack;
-        }
+    EXPECT_DELAY_MS(timeout_ms);
+    if (hal.rcout->serial_read_bytes(&ack, 1, timeout_ms * 1000) == 1) {
+        return ack;
     }
     // return brNONE, meaning no ACK received in the timeout
     return brNONE;
@@ -743,7 +784,7 @@ bool AP_BLHeli::BL_ReadA(uint8_t cmd, uint8_t *buf, uint16_t n)
  */
 bool AP_BLHeli::BL_ConnectEx(void)
 {
-    debug("BL_ConnectEx %u/%u at %u", blheli.chan, num_motors, motor_map[blheli.chan]);
+    debug("BL_ConnectEx %u/%u at %u", blheli.chan, num_motors, blheli_chan_to_output_chan(blheli.chan));
     setDisconnected();
     const uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
     if (!BL_SendBuf(BootInit, 21)) {
@@ -840,15 +881,22 @@ void AP_BLHeli::BL_SendCMDRunRestartBootloader(void)
 uint8_t AP_BLHeli::BL_SendCMDSetBuffer(const uint8_t *buf, uint16_t nbytes)
 {
     uint8_t sCMD[] = {CMD_SET_BUFFER, 0, uint8_t(nbytes>>8), uint8_t(nbytes&0xff)};
+    if (nbytes == 0) {
+        // set high byte since 0 bytes == 256 bytes in this protocol
+        sCMD[2] = 1;
+    }
     if (!BL_SendBuf(sCMD, 4)) {
+        debug_console("BL_SendCMDSetBuffer send cmd failed");
         return false;
     }
-    uint8_t ack;
-    if ((ack = BL_GetACK()) != brNONE) {
+    uint8_t ack = BL_GetACK(5); // match betaflight timing
+    // generally no ack returned for CMD_SET_BUFFER when flashing firmware
+    if (ack != brNONE && ack != brSUCCESS) {
         debug("BL_SendCMDSetBuffer ack failed 0x%02x", ack);
         blheli.ack = ACK_D_GENERAL_ERROR;
         return false;
     }
+
     if (!BL_SendBuf(buf, nbytes)) {
         debug("BL_SendCMDSetBuffer send failed");
         blheli.ack = ACK_D_GENERAL_ERROR;
@@ -861,20 +909,27 @@ bool AP_BLHeli::BL_WriteA(uint8_t cmd, const uint8_t *buf, uint16_t nbytes, uint
 {
     if (BL_SendCMDSetAddress()) {
         if (!BL_SendCMDSetBuffer(buf, nbytes)) {
+            debug_console("BL_SendCMDSetBuffer failed\n");
             blheli.ack = ACK_D_GENERAL_ERROR;
             return false;
         }
         uint8_t sCMD[] = {cmd, 0x01};
         if (!BL_SendBuf(sCMD, 2)) {
+            debug_console("BL_SendBuf failed\n");
             return false;
         }
-        return (BL_GetACK(timeout_ms) == brSUCCESS);
+        uint8_t ack = BL_GetACK(timeout_ms);
+        if (ack != brSUCCESS) {
+            debug_console("BL_GetACK failed 0x%x\n", ack);
+        }
+        return (ack == brSUCCESS);
     }
+    debug_console("BL_SendCMDSetAddress failed\n");
     blheli.ack = ACK_D_GENERAL_ERROR;
     return false;
 }
 
-uint8_t AP_BLHeli::BL_WriteFlash(const uint8_t *buf, uint16_t n)
+bool AP_BLHeli::BL_WriteFlash(const uint8_t *buf, uint16_t n)
 {
     return BL_WriteA(CMD_PROG_FLASH, buf, n, 500);
 }
@@ -952,8 +1007,7 @@ void AP_BLHeli::blheli_process_command(void)
         msp.escMode = PROTOCOL_NONE;
         uint8_t b = 0;
         blheli_send_reply(&b, 1);
-        hal.rcout->serial_end();
-        serial_start_ms = 0;
+        serial_end();
         if (motors_disabled) {
             motors_disabled = false;
             SRV_Channels::set_disabled_channel_mask(motors_disabled_mask);
@@ -1079,7 +1133,13 @@ void AP_BLHeli::blheli_process_command(void)
         case imSIL_BLB:
         case imATM_BLB:
         case imARM_BLB: {
-            BL_WriteFlash(buf, nbytes);
+            if (!BL_WriteFlash(buf, nbytes)) {
+                // For reasons unknown BL_WriteFlash can bork the DMA engine, make some attempt to get
+                // back to a sane state if this happens. We can't call serial_end() as that will 
+                // restart dshot output
+                debug_console("cmd_DeviceWrite failed 0x%x: %u bytes", blheli.address, nbytes);
+                hal.rcout->serial_reset(motor_mask);
+            }
             break;
         }
         case imSK: {
@@ -1175,8 +1235,7 @@ bool AP_BLHeli::process_input(uint8_t b)
     if (msp.escMode == PROTOCOL_4WAY && blheli.state == BLHELI_IDLE && b == '$') {
         debug("Change to MSP mode");
         msp.escMode = PROTOCOL_NONE;
-        hal.rcout->serial_end();
-        serial_start_ms = 0;
+        serial_end();
     }
     if (msp.escMode != PROTOCOL_4WAY && msp.state == MSP_IDLE && b == '/') {
         debug("Change to BLHeli mode");
@@ -1269,10 +1328,9 @@ void AP_BLHeli::run_connection_test(uint8_t chan)
             break;
         }
     }
-    hal.rcout->serial_end();
+    serial_end();
     SRV_Channels::set_disabled_channel_mask(motors_disabled_mask);
     motors_disabled = false;
-    serial_start_ms = 0;
     blheli.chan = saved_chan;
     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: Test %s", passed?"PASSED":"FAILED");
     debug_uart = nullptr;
@@ -1298,8 +1356,7 @@ void AP_BLHeli::update(void)
         // we're not processing requests any more, shutdown serial
         // output
         if (serial_start_ms) {
-            hal.rcout->serial_end();
-            serial_start_ms = 0;
+            serial_end();
         }
         if (motors_disabled) {
             motors_disabled = false;
@@ -1325,6 +1382,12 @@ void AP_BLHeli::update(void)
     }
 }
 
+void AP_BLHeli::serial_end()
+{
+    hal.rcout->serial_end(motor_mask);
+    serial_start_ms = 0;
+}
+
 /*
   Initialize BLHeli, called by SRV_Channels::init()
   Used to install protocol handler
@@ -1428,13 +1491,13 @@ void AP_BLHeli::init(uint32_t mask, AP_HAL::RCOutput::output_mode otype)
 #endif
     // add motors from channel mask
     for (uint8_t i=0; i<16 && num_motors < max_motors; i++) {
-        if (mask & (1U<<i)) {
+        if (digital_mask & (1U<<i)) {
             motor_map[num_motors] = i;
             num_motors++;
         }
     }
     motor_mask = mask;
-    debug("ESC: %u motors mask=0x%08lx", num_motors, mask);
+    debug("ESC: %u motors mask=0x%08lx", num_motors, digital_mask);
 
     // check if we have a combination of reversible and normal
     mixed_type = (mask != (mask & channel_reversible_mask.get())) && (channel_reversible_mask.get() != 0);

libraries/AP_BLHeli/AP_BLHeli.h

@@ -257,9 +257,11 @@ class AP_BLHeli : public AP_ESC_Telem_Backend {
     uint32_t last_telem_byte_read_us;
     int8_t last_control_port;
 
+    void serial_end();
     bool msp_process_byte(uint8_t c);
     void blheli_crc_update(uint8_t c);
     bool blheli_4way_process_byte(uint8_t c);
+    uint8_t blheli_chan_to_output_chan(uint8_t motor);
     void msp_send_ack(uint8_t cmd);
     void msp_send_reply(uint8_t cmd, const uint8_t *buf, uint8_t len);
     void putU16(uint8_t *b, uint16_t v);
@@ -282,7 +284,7 @@ class AP_BLHeli : public AP_ESC_Telem_Backend {
     void BL_SendCMDRunRestartBootloader(void);
     uint8_t BL_SendCMDSetBuffer(const uint8_t *buf, uint16_t nbytes);
     bool BL_WriteA(uint8_t cmd, const uint8_t *buf, uint16_t nbytes, uint32_t timeout);
-    uint8_t BL_WriteFlash(const uint8_t *buf, uint16_t n);
+    bool BL_WriteFlash(const uint8_t *buf, uint16_t n);
     bool BL_VerifyFlash(const uint8_t *buf, uint16_t n);
     void blheli_process_command(void);
     void run_connection_test(uint8_t chan);

libraries/AP_HAL/RCOutput.h

@@ -194,16 +194,22 @@ class AP_HAL::RCOutput {
       read a series of bytes from a port, using serial parameters from serial_setup_output()
       return the number of bytes read. This is a blocking call
      */
-    virtual uint16_t serial_read_bytes(uint8_t *buf, uint16_t len) { return 0; }
+    virtual uint16_t serial_read_bytes(uint8_t *buf, uint16_t len, uint32_t timeout_us) { return 0; }
 
     /*
       stop serial output. This restores the previous output mode for
       the channel and any other channels that were stopped by
       serial_setup_output()
      */
-    virtual void serial_end(void) {}
+    virtual void serial_end(uint32_t chanmask) {}
 
     /*
+      reset serial output. This re-initializes the DMA configuration to that configured by
+      serial_setup_output()
+     */
+    virtual void serial_reset(uint32_t chanmask) {}
+
+     /*
       output modes. Allows for support of PWM, oneshot and dshot 
     */
     // this enum is used by BLH_OTYPE and ESC_PWM_TYPE on AP_Periph