diff --git a/src/main/config/config.c b/src/main/config/config.c index fac719dd48..32f5299300 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -308,7 +308,7 @@ void resetSerialConfig(serialConfig_t *serialConfig) for (index = 0; index < SERIAL_PORT_COUNT; index++) { serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index]; - serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200; + serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_500000; serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600; serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO; serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index e5ef5da9f2..8b5fcfc855 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -85,7 +85,7 @@ const serialPortIdentifier_e serialPortIdentifiers[SERIAL_PORT_COUNT] = { static uint8_t serialPortCount; -const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000}; // see baudRate_e +const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 500000, 1000000}; // see baudRate_e #define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0])) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 924f582323..13845446a1 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -46,6 +46,8 @@ typedef enum { BAUD_115200, BAUD_230400, BAUD_250000, + BAUD_500000, + BAUD_1000000, } baudRate_e; extern const uint32_t baudRates[]; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 088eafbc8c..84cec6466e 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -195,6 +195,10 @@ STATIC_UNIT_TESTED bufWriter_t *writer; #define RATEPROFILE_MASK (1 << 7) +#define JUMBO_FRAME_SIZE_LIMIT 255 + +#define DATAFLASH_BUFFER_SIZE 4096 + static void serialize8(uint8_t a) { bufWriterAppend(writer, a); @@ -232,7 +236,7 @@ static uint32_t read32(void) return t; } -static void headSerialResponse(uint8_t err, uint8_t responseBodySize) +static void headSerialResponse(uint8_t err, uint16_t responseBodySize) { serialBeginWrite(mspSerialPort); @@ -240,11 +244,18 @@ static void headSerialResponse(uint8_t err, uint8_t responseBodySize) serialize8('M'); serialize8(err ? '!' : '>'); currentPort->checksum = 0; // start calculating a new checksum - serialize8(responseBodySize); + if (responseBodySize < JUMBO_FRAME_SIZE_LIMIT) { + serialize8(responseBodySize); + } else { + serialize8(JUMBO_FRAME_SIZE_LIMIT); + } serialize8(currentPort->cmdMSP); + if (responseBodySize >= JUMBO_FRAME_SIZE_LIMIT) { + serialize16(responseBodySize); + } } -static void headSerialReply(uint8_t responseBodySize) +static void headSerialReply(uint16_t responseBodySize) { headSerialResponse(0, responseBodySize); } @@ -400,25 +411,38 @@ static void serializeDataflashSummaryReply(void) } #ifdef USE_FLASHFS -static void serializeDataflashReadReply(uint32_t address, uint8_t size) +static void serializeDataflashReadReply(uint32_t address, uint16_t size, bool useLegacyFormat) { - uint8_t buffer[128]; - int bytesRead; + static uint8_t buffer[DATAFLASH_BUFFER_SIZE]; if (size > sizeof(buffer)) { size = sizeof(buffer); } - headSerialReply(4 + size); - - serialize32(address); - // bytesRead will be lower than that requested if we reach end of volume - bytesRead = flashfsReadAbs(address, buffer, size); + int bytesRead = flashfsReadAbs(address, buffer, size); - for (int i = 0; i < bytesRead; i++) { + if (useLegacyFormat) { + headSerialReply(sizeof(uint32_t) + size); + + serialize32(address); + } else { + headSerialReply(sizeof(uint32_t) + sizeof(uint16_t) + bytesRead); + + serialize32(address); + serialize16(bytesRead); + } + + int i; + for (i = 0; i < bytesRead; i++) { serialize8(buffer[i]); } + + if (useLegacyFormat) { + for (; i < size; i++) { + serialize8(0); + } + } } #endif @@ -1162,8 +1186,17 @@ static bool processOutCommand(uint8_t cmdMSP) case MSP_DATAFLASH_READ: { uint32_t readAddress = read32(); + uint16_t readLength; + bool useLegacyFormat; + if (currentPort->dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) { + readLength = read16(); + useLegacyFormat = false; + } else { + readLength = 128; + useLegacyFormat = true; + } - serializeDataflashReadReply(readAddress, 128); + serializeDataflashReadReply(readAddress, readLength, useLegacyFormat); } break; #endif