- MultiFlasher support for SimonK escs

src/main/io/serial_msp.c

@@ -95,6 +95,9 @@
 #ifdef USE_SERIAL_1WIRE
 #include "io/serial_1wire.h"
 #endif
+#ifdef USE_ESCSERIAL
+#include "drivers/serial_escserial.h"
+#endif
 static serialPort_t *mspSerialPort;
 
 extern uint16_t cycleTime; // FIXME dependency on mw.c
@@ -1841,6 +1844,50 @@ static bool processInCommand(void)
         }
         break;
 #endif
+
+#ifdef USE_ESCSERIAL
+    case MSP_SET_ESCSERIAL:
+        // get channel number
+        i = read8();
+        // we do not give any data back, assume channel number is transmitted OK
+        if (i == 0xFF) {
+            // 0xFF -> preinitialize the Passthrough
+            // switch all motor lines HI
+            escSerialInitialize();
+
+            // and come back right afterwards
+            // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
+            // bootloader mode before try to connect any ESC
+        }
+        else {
+            // Check for channel number 1..USABLE_TIMER_CHANNEL_COUNT-1
+            if ((i > 0) && (i < USABLE_TIMER_CHANNEL_COUNT)) {
+                // because we do not come back after calling escEnablePassthrough
+                // proceed with a success reply first
+                headSerialReply(0);
+                tailSerialReply();
+			 // flush the transmit buffer
+			 bufWriterFlush(writer);
+                // wait for all data to send
+                while (!isSerialTransmitBufferEmpty(mspSerialPort)) {
+                    delay(50);
+                }
+                // Start to activate here
+                // motor 1 => index 0
+                escEnablePassthrough(mspSerialPort,i,0); //sk blmode
+                // MPS uart is active again
+            } else {
+                // ESC channel higher than max. allowed
+                // rem: BLHeliSuite will not support more than 8
+                headSerialError(0);
+            }
+            // proceed as usual with MSP commands
+            // and wait to switch to next channel
+            // rem: App needs to call MSP_BOOT to deinitialize Passthrough
+        }
+        break;
+#endif
+
     default:
         // we do not know how to handle the (valid) message, indicate error MSP $M!
         return false;