diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index c153c92c56..7a2aaa0d40 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -103,6 +103,7 @@ #include "msp/msp_box.h" #include "msp/msp_protocol.h" +#include "msp/msp_protocol_v2_common.h" #include "msp/msp_serial.h" #include "osd/osd.h" @@ -557,7 +558,7 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin * Returns true if the command was processd, false otherwise. * May set mspPostProcessFunc to a function to be called once the command has been processed */ -static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) +static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) { UNUSED(mspPostProcessFn); @@ -959,7 +960,7 @@ static bool mspCommonProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProce return true; } -static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) +static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) { bool unsupportedCommand = false; @@ -1478,6 +1479,27 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex); } break; + case MSP2_COMMON_SERIAL_CONFIG: { + uint8_t count = 0; + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + if (serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) { + count++; + } + } + sbufWriteU8(dst, count); + for (int i = 0; i < SERIAL_PORT_COUNT; i++) { + if (!serialIsPortAvailable(serialConfig()->portConfigs[i].identifier)) { + continue; + }; + sbufWriteU8(dst, serialConfig()->portConfigs[i].identifier); + sbufWriteU32(dst, serialConfig()->portConfigs[i].functionMask); + sbufWriteU8(dst, serialConfig()->portConfigs[i].msp_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].gps_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].telemetry_baudrateIndex); + sbufWriteU8(dst, serialConfig()->portConfigs[i].blackbox_baudrateIndex); + } + break; + } #ifdef USE_LED_STRIP_STATUS_MODE case MSP_LED_COLORS: @@ -1853,7 +1875,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) return !unsupportedCommand; } -static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) +static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn) { switch (cmdMSP) { @@ -2039,7 +2061,7 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) } #endif -static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src) +static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src) { uint32_t i; uint8_t value; @@ -3048,6 +3070,38 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, } } break; + case MSP2_COMMON_SET_SERIAL_CONFIG: { + if (dataSize < 1) { + return MSP_RESULT_ERROR; + } + unsigned count = sbufReadU8(src); + unsigned portConfigSize = (dataSize - 1) / count; + unsigned expectedPortSize = sizeof(uint8_t) + sizeof(uint32_t) + (sizeof(uint8_t) * 4); + if (portConfigSize < expectedPortSize) { + return MSP_RESULT_ERROR; + } + for (unsigned ii = 0; ii < count; ii++) { + unsigned start = sbufBytesRemaining(src); + uint8_t identifier = sbufReadU8(src); + serialPortConfig_t *portConfig = serialFindPortConfigurationMutable(identifier); + + if (!portConfig) { + return MSP_RESULT_ERROR; + } + + portConfig->identifier = identifier; + portConfig->functionMask = sbufReadU32(src); + portConfig->msp_baudrateIndex = sbufReadU8(src); + portConfig->gps_baudrateIndex = sbufReadU8(src); + portConfig->telemetry_baudrateIndex = sbufReadU8(src); + portConfig->blackbox_baudrateIndex = sbufReadU8(src); + // Skip unknown bytes + while (start - sbufBytesRemaining(src) < portConfigSize && sbufBytesRemaining(src)) { + sbufReadU8(src); + } + } + break; + } #ifdef USE_LED_STRIP_STATUS_MODE case MSP_SET_LED_COLORS: @@ -3170,7 +3224,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, return MSP_RESULT_ACK; } -static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) +static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) { UNUSED(mspPostProcessFn); const unsigned int dataSize = sbufBytesRemaining(src); @@ -3425,7 +3479,7 @@ mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPa int ret = MSP_RESULT_ACK; sbuf_t *dst = &reply->buf; sbuf_t *src = &cmd->buf; - const uint8_t cmdMSP = cmd->cmd; + const int16_t cmdMSP = cmd->cmd; // initialize reply by default reply->cmd = cmd->cmd; diff --git a/src/main/msp/msp_protocol_v2_common.h b/src/main/msp/msp_protocol_v2_common.h new file mode 100644 index 0000000000..2031df2bbe --- /dev/null +++ b/src/main/msp/msp_protocol_v2_common.h @@ -0,0 +1,22 @@ +/* + * This file is part of Cleanflight, Betaflight and INAV. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight, Betaflight and INAV are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#define MSP2_COMMON_SERIAL_CONFIG 0x1009 +#define MSP2_COMMON_SET_SERIAL_CONFIG 0x100A