From 3750ed2925ccb6794e66240e84e84635e207d206 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 28 Oct 2019 16:03:07 +1100
Subject: [PATCH] AP_BLHeli: fixed critical errors caused by BLHeli code

---
 libraries/AP_BLHeli/AP_BLHeli.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp
index 998e910db2..08686aea62 100644
--- a/libraries/AP_BLHeli/AP_BLHeli.cpp
+++ b/libraries/AP_BLHeli/AP_BLHeli.cpp
@@ -421,6 +421,7 @@ void AP_BLHeli::msp_process_command(void)
         debug("MSP_SET_MOTOR %u", nmotors);
         SRV_Channels::set_disabled_channel_mask(0xFFFF);
         motors_disabled = true;
+        EXPECT_DELAY_MS(1000);
         hal.rcout->cork();
         for (uint8_t i = 0; i < nmotors; i++) {
             if (i >= num_motors) {
@@ -524,6 +525,7 @@ bool AP_BLHeli::BL_SendBuf(const uint8_t *buf, uint16_t len)
     if (blheli.chan >= num_motors) {
         return false;
     }
+    EXPECT_DELAY_MS(1000);
     if (!hal.rcout->serial_setup_output(motor_map[blheli.chan], 19200, motor_mask)) {
         blheli.ack = ACK_D_GENERAL_ERROR;
         return false;
@@ -557,6 +559,7 @@ bool AP_BLHeli::BL_ReadBuf(uint8_t *buf, uint16_t len)
 {
     bool check_crc = isMcuConnected() && len > 0;
     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);
     debug("BL_ReadBuf %u -> %u", len, n);
     if (req_bytes != n) {
@@ -594,6 +597,7 @@ 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;
@@ -1134,6 +1138,7 @@ void AP_BLHeli::run_connection_test(uint8_t chan)
     run_test.set_and_notify(0);
     bool passed = false;
     for (uint8_t tries=0; tries<5; tries++) {
+        EXPECT_DELAY_MS(3000);
         blheli.ack = ACK_OK;
         setDisconnected();
         if (BL_ConnectEx()) {