diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 71533e6d6ad..cb88e221b15 100644 --- a/src/main/fc/cli.c +++ b/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'"); diff --git a/src/main/fc/cli.h b/src/main/fc/cli.h index 8286d448f15..d35704e1751 100644 --- a/src/main/fc/cli.h +++ b/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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 36d91b0bbb6..49850b32411 100644 --- a/src/main/fc/fc_msp.c +++ b/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; diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index dfa560b0a07..503e22cbdc2 100644 --- a/src/main/msp/msp_protocol.h +++ b/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 diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index d3e7d14585a..88df8c13b9b 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/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 diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index 1a501627379..65308217347 100644 --- a/src/main/msp/msp_serial.c +++ b/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(); diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 0c495096808..32278f3e3e0 100644 --- a/src/main/msp/msp_serial.h +++ b/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);