Workaround for MSP Connections over BLE

src/main/fc/cli.c

@@ -140,6 +140,7 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128];
 
 static char cliBuffer[64];
 static uint32_t bufferIndex = 0;
+static bool sendDelay = false;
 
 #if defined(USE_ASSERT)
 static void cliAssert(char *cmdline);
@@ -199,6 +200,7 @@ static const char * const *sensorHardwareNames[] = {
         table_rangefinder_hardware,
 #else
         NULL,
+
 #endif
 #ifdef USE_PITOT
         table_pitot_hardware,
@@ -215,7 +217,11 @@ static const char * const *sensorHardwareNames[] = {
 static void cliPrint(const char *str)
 {
     while (*str) {
-        bufWriterAppend(cliWriter, *str++);
+        bufWriterAppend(cliWriter, *str++); 
+    }
+    // Add some delay for BLE, so that the ble adapter can flush its buffer
+    if (sendDelay) {
+        delay(30);
     }
 }
 
@@ -4071,7 +4077,7 @@ void cliProcess(void)
     }
 }
 
-void cliEnter(serialPort_t *serialPort)
+void cliEnter(serialPort_t *serialPort, bool delay)
 {
     if (cliMode) {
         return;
@@ -4081,6 +4087,7 @@ void cliEnter(serialPort_t *serialPort)
     cliPort = serialPort;
     setPrintfSerialPort(cliPort);
     cliWriter = bufWriterInit(cliWriteBuffer, sizeof(cliWriteBuffer), (bufWrite_t)serialWriteBufShim, serialPort);
+    sendDelay = delay;
 
 #ifndef CLI_MINIMAL_VERBOSITY
     cliPrintLine("\r\nEntering CLI Mode, type 'exit' to return, or 'help'");

src/main/fc/cli.h

@@ -23,4 +23,4 @@ struct serialConfig_s;
 void cliInit(const struct serialConfig_s *serialConfig);
 void cliProcess(void);
 struct serialPort_s;
-void cliEnter(struct serialPort_s *serialPort);
+void cliEnter(struct serialPort_s *serialPort, bool delay);

src/main/fc/fc_msp.c

@@ -3039,6 +3039,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             return MSP_RESULT_ERROR;
         }
         break;
+    case MSP2_INAV_SET_MSP_OPTIONS:
+        mspSendChunkSize = sbufReadU16(src);
+        mspChunkDelay = sbufReadU16(src);
+        break;
 
     default:
         return MSP_RESULT_ERROR;

src/main/msp/msp_protocol.h

@@ -60,7 +60,7 @@
 #define MSP_PROTOCOL_VERSION                0   // Same version over MSPv1 & MSPv2 - message format didn't change and it backward compatible
 
 #define API_VERSION_MAJOR                   2   // increment when major changes are made
-#define API_VERSION_MINOR                   4   // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
+#define API_VERSION_MINOR                   5   // increment when any change is made, reset to zero when major changes are released after changing API_VERSION_MAJOR
 
 #define API_VERSION_LENGTH                  2
 

src/main/msp/msp_protocol_v2_inav.h

@@ -80,3 +80,4 @@
 #define MSP2_INAV_SET_SAFEHOME                  0x2039
 
 #define MSP2_INAV_MISC2                         0x203A
+#define MSP2_INAV_SET_MSP_OPTIONS               0x203B

src/main/msp/msp_serial.c

@@ -38,7 +38,8 @@
 #include "msp/msp_serial.h"
 
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
-
+uint16_t mspSendChunkSize = 0;
+uint16_t mspChunkDelay = 0;
 
 void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
 {
@@ -266,7 +267,7 @@ static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int l
 }
 
 #define JUMBO_FRAME_SIZE_LIMIT 255
-static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, const uint8_t * data, int dataLen, const uint8_t * crc, int crcLen)
+static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, uint8_t * data, int dataLen, uint8_t * crc, int crcLen)
 {
     // MSP port might be turned into a CLI port, which will make
     // msp->port become NULL.
@@ -287,13 +288,23 @@ static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, c
     if (!isSerialTransmitBufferEmpty(port) && ((int)serialTxBytesFree(port) < totalFrameLength))
         return 0;
 
-    // Transmit frame
     serialBeginWrite(port);
     serialWriteBuf(port, hdr, hdrLen);
-    serialWriteBuf(port, data, dataLen);
-    serialWriteBuf(port, crc, crcLen);
+    // BLE dont't like large frames, send in chunks
+    if (msp->chunk.sendSize && totalFrameLength > msp->chunk.sendSize) {
+        uint16_t chunkPayloadLen =  msp->chunk.sendSize - hdrLen;
+        serialWriteBuf(port, data, chunkPayloadLen);
+        msp->chunk.pendingDataSize = dataLen - chunkPayloadLen;
+        msp->chunk.dataBuf = data + chunkPayloadLen;
+        msp->chunk.crcBuf = crc;
+        msp->chunk.crcSize = crcLen;
+
+    } else {
+        serialWriteBuf(port, data, dataLen);
+        serialWriteBuf(port, crc, crcLen);     
+    }
     serialEndWrite(port);
-
+    msp->lastActivityMs = millis();
     return totalFrameLength;
 }
 
@@ -410,6 +421,12 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
 
     mspPostProcessFnPtr mspPostProcessFn = NULL;
     const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
+    if (mspSendChunkSize) {
+        msp->chunk.sendSize = mspSendChunkSize;
+        msp->chunk.sendDelay = mspChunkDelay;
+        mspSendChunkSize = 0;
+        mspChunkDelay = 0;
+    }
 
     if (status != MSP_RESULT_NO_REPLY) {
         sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
@@ -448,7 +465,7 @@ static void mspProcessPendingRequest(mspPort_t * mspPort)
         case MSP_PENDING_CLI:
             if (!cliMode) {
                 // When we enter CLI mode - disable this MSP port. Don't care about preserving the port since CLI can only be exited via reboot
-                cliEnter(mspPort->port);
+                cliEnter(mspPort->port, mspPort->chunk.sendDelay);
                 mspPort->port = NULL;
             }
             break;
@@ -462,6 +479,33 @@ void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e
 {
     mspPostProcessFnPtr mspPostProcessFn = NULL;
 
+    // Send pending chunks first
+    if (mspPort->chunk.sendSize && mspPort->chunk.pendingDataSize) {        
+        if (millis() - mspPort->lastActivityMs < mspPort->chunk.sendDelay ) { // Add some delay between the chunks, othewise it will be send as one  
+            return;
+        }
+        serialBeginWrite(mspPort->port);      
+        uint16_t sendSize = mspPort->chunk.pendingDataSize + mspPort->chunk.crcSize;
+        if (sendSize > mspPort->chunk.sendSize) {
+        if (mspPort->chunk.pendingDataSize < mspPort->chunk.sendSize) {
+            sendSize = mspPort->chunk.pendingDataSize;
+            } else {
+                sendSize = mspPort->chunk.sendSize;
+            }
+        serialWriteBuf(mspPort->port, mspPort->chunk.dataBuf, sendSize);
+        serialEndWrite(mspPort->port);
+        mspPort->chunk.dataBuf += sendSize;
+        mspPort->chunk.pendingDataSize -= sendSize;
+        mspPort->lastActivityMs = millis();
+        return;
+        } else { 
+            serialWriteBuf(mspPort->port, mspPort->chunk.dataBuf, mspPort->chunk.pendingDataSize);
+            serialWriteBuf(mspPort->port, mspPort->chunk.crcBuf, mspPort->chunk.crcSize);
+            mspPort->chunk.pendingDataSize = 0;
+        }      
+        serialEndWrite(mspPort->port);
+    } 
+
     if (serialRxBytesWaiting(mspPort->port)) {
         // There are bytes incoming - abort pending request
         mspPort->lastActivityMs = millis();

src/main/msp/msp_serial.h

@@ -83,6 +83,16 @@ typedef struct __attribute__((packed)) {
 #define MSP_MAX_HEADER_SIZE     9
 
 struct serialPort_s;
+
+typedef struct mspChunk_s {
+    uint8_t *dataBuf;
+    uint8_t *crcBuf;
+    uint8_t crcSize;
+    uint16_t sendSize;
+    uint16_t sendDelay;
+    uint16_t pendingDataSize;
+} mspChunk_t;
+
 typedef struct mspPort_s {
     struct serialPort_s *port; // null when port unused.
     timeMs_t lastActivityMs;
@@ -96,9 +106,13 @@ typedef struct mspPort_s {
     uint16_t cmdMSP;
     uint8_t checksum1;
     uint8_t checksum2;
+    mspChunk_t chunk;
 } mspPort_t;
 
 
+extern uint16_t mspSendChunkSize;
+extern uint16_t mspChunkDelay;
+
 void mspSerialInit(void);
 void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort);
 void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn);