From 38ea1296ca1ee96fad8b4fc99b249acd2d22dbde Mon Sep 17 00:00:00 2001
From: 4712 <4712@outlook.de>
Date: Tue, 20 Oct 2020 14:43:04 +0200
Subject: [PATCH] Update serial-4way-if to v20005

Reservation of an MCU_ID area to simplificate adding of upcoming BLHeli_32 MCU.
---
 libraries/AP_BLHeli/AP_BLHeli.cpp          | 21 +++++++++------------
 libraries/AP_BLHeli/blheli_4way_protocol.h |  2 +-
 2 files changed, 10 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp
index 38b5e98991..652235fa37 100644
--- a/libraries/AP_BLHeli/AP_BLHeli.cpp
+++ b/libraries/AP_BLHeli/AP_BLHeli.cpp
@@ -741,19 +741,16 @@ bool AP_BLHeli::BL_ConnectEx(void)
         blheli.interface_mode[blheli.chan] = imSIL_BLB;
         debug("Interface type imSIL_BLB");
         break;
-    case 0x1F06:
-    case 0x3306:
-    case 0x3406:
-    case 0x3506:
-    case 0x2B06:
-    case 0x4706:
-        blheli.interface_mode[blheli.chan] = imARM_BLB;
-        debug("Interface type imARM_BLB");
-        break;
     default:
-        blheli.ack = ACK_D_GENERAL_ERROR;        
-        debug("Unknown interface type 0x%04x", *devword);
-        break;
+        // BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06
+        if ((blheli.deviceInfo[blheli.chan][1] > 0x00) && (blheli.deviceInfo[blheli.chan][1] < 0x90) && (blheli.deviceInfo[blheli.chan][0] == 0x06)) {
+            blheli.interface_mode[blheli.chan] = imARM_BLB;
+            debug("Interface type imARM_BLB");
+        } else {
+            blheli.ack = ACK_D_GENERAL_ERROR;
+            debug("Unknown interface type 0x%04x", *devword);
+            break;
+        }
     }
     blheli.deviceInfo[blheli.chan][3] = blheli.interface_mode[blheli.chan];
     if (blheli.interface_mode[blheli.chan] != 0) {
diff --git a/libraries/AP_BLHeli/blheli_4way_protocol.h b/libraries/AP_BLHeli/blheli_4way_protocol.h
index bfb9250bb3..1f436e8286 100644
--- a/libraries/AP_BLHeli/blheli_4way_protocol.h
+++ b/libraries/AP_BLHeli/blheli_4way_protocol.h
@@ -118,7 +118,7 @@
  */
 #define SERIAL_4WAY_VER_MAIN 20
 #define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0
-#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 03
+#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 05
 
 #define SERIAL_4WAY_PROTOCOL_VER 107
 // *** end