From deeabdbca32b192409350eaa1efee05fc2d4a2e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 8 Dec 2019 22:48:57 +0000 Subject: [PATCH] Add support for serial passthrough via MSP - Rename MSP_SET_4WAY_IF to MSP_SET_PASSTHROUGH - Keep backwards compatibility with existing uses of the command - Add 2 new values for the passthrough mode: one that sets up a serial port given its ID and another one that sets it up from a function ID. --- src/main/msp/msp.c | 99 ++++++++++++++++++++++++++++--------- src/main/msp/msp_protocol.h | 2 +- 2 files changed, 76 insertions(+), 25 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 8a4beb7e3b..acd8b7e926 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -172,6 +172,19 @@ typedef enum { MSP_FLASHFS_FLAG_SUPPORTED = 2 } mspFlashFsFlags_e; +typedef enum { + MSP_PASSTHROUGH_ESC_SIMONK = PROTOCOL_SIMONK, + MSP_PASSTHROUGH_ESC_BLHELI = PROTOCOL_BLHELI, + MSP_PASSTHROUGH_ESC_KISS = PROTOCOL_KISS, + MSP_PASSTHROUGH_ESC_KISSALL = PROTOCOL_KISSALL, + MSP_PASSTHROUGH_ESC_CASTLE = PROTOCOL_CASTLE, + + MSP_PASSTHROUGH_SERIAL_ID = 0xFD, + MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE, + + MSP_PASSTHROUGH_ESC_4WAY = 0xFF, +} mspPassthroughType_e; + #define RATEPROFILE_MASK (1 << 7) #define RTC_NOT_SUPPORTED 0xff @@ -209,33 +222,73 @@ static bool mspIsMspArmingEnabled(void) return mspArmingDisableFlags == 0; } -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE -#define ESC_4WAY 0xff +#define MSP_PASSTHROUGH_ESC_4WAY 0xff -uint8_t escMode; -uint8_t escPortIndex; +static uint8_t mspPassthroughMode; +static uint8_t mspPassthroughArgument; #ifdef USE_ESCSERIAL static void mspEscPassthroughFn(serialPort_t *serialPort) { - escEnablePassthrough(serialPort, &motorConfig()->dev, escPortIndex, escMode); + escEnablePassthrough(serialPort, &motorConfig()->dev, mspPassthroughArgument, mspPassthroughMode); } #endif -static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) +static serialPort_t *mspFindPassthroughSerialPort(void) +{ + serialPortUsage_t *portUsage = NULL; + + switch (mspPassthroughMode) { + case MSP_PASSTHROUGH_SERIAL_ID: + { + portUsage = findSerialPortUsageByIdentifier(mspPassthroughArgument); + break; + } + case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID: + { + const serialPortConfig_t *portConfig = findSerialPortConfig(1 << mspPassthroughArgument); + if (portConfig) { + portUsage = findSerialPortUsageByIdentifier(portConfig->identifier); + } + break; + } + } + return portUsage ? portUsage->serialPort : NULL; +} + +static void mspSerialPassthroughFn(serialPort_t *serialPort) +{ + serialPort_t *passthroughPort = mspFindPassthroughSerialPort(); + if (passthroughPort && serialPort) { + serialPassthrough(passthroughPort, serialPort, NULL, NULL); + } +} + +static void mspFcSetPassthroughCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn) { const unsigned int dataSize = sbufBytesRemaining(src); if (dataSize == 0) { // Legacy format - - escMode = ESC_4WAY; + mspPassthroughMode = MSP_PASSTHROUGH_ESC_4WAY; } else { - escMode = sbufReadU8(src); - escPortIndex = sbufReadU8(src); + mspPassthroughMode = sbufReadU8(src); + mspPassthroughArgument = sbufReadU8(src); } - switch (escMode) { - case ESC_4WAY: + switch (mspPassthroughMode) { + case MSP_PASSTHROUGH_SERIAL_ID: + case MSP_PASSTHROUGH_SERIAL_FUNCTION_ID: + if (mspFindPassthroughSerialPort()) { + if (mspPostProcessFn) { + *mspPostProcessFn = mspSerialPassthroughFn; + } + sbufWriteU8(dst, 1); + } else { + sbufWriteU8(dst, 0); + } + break; +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + case MSP_PASSTHROUGH_ESC_4WAY: // get channel number // switch all motor lines HI // reply with the count of ESC found @@ -247,12 +300,12 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr break; #ifdef USE_ESCSERIAL - case PROTOCOL_SIMONK: - case PROTOCOL_BLHELI: - case PROTOCOL_KISS: - case PROTOCOL_KISSALL: - case PROTOCOL_CASTLE: - if (escPortIndex < getMotorCount() || (escMode == PROTOCOL_KISS && escPortIndex == ALL_MOTORS)) { + case MSP_PASSTHROUGH_ESC_SIMONK: + case MSP_PASSTHROUGH_ESC_BLHELI: + case MSP_PASSTHROUGH_ESC_KISS: + case MSP_PASSTHROUGH_ESC_KISSALL: + case MSP_PASSTHROUGH_ESC_CASTLE: + if (mspPassthroughArgument < getMotorCount() || (mspPassthroughMode == MSP_PASSTHROUGH_ESC_KISS && mspPassthroughArgument == ALL_MOTORS)) { sbufWriteU8(dst, 1); if (mspPostProcessFn) { @@ -262,12 +315,12 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr break; } FALLTHROUGH; -#endif +#endif // USE_ESCSERIAL +#endif //USE_SERIAL_4WAY_BLHELI_INTERFACE default: sbufWriteU8(dst, 0); } } -#endif //USE_SERIAL_4WAY_BLHELI_INTERFACE // TODO: Remove the pragma once this is called from unconditional code #pragma GCC diagnostic ignored "-Wunused-function" @@ -3369,11 +3422,9 @@ mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPa ret = MSP_RESULT_ACK; } else if ((ret = mspFcProcessOutCommandWithArg(srcDesc, cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) { /* ret */; -#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE - } else if (cmdMSP == MSP_SET_4WAY_IF) { - mspFc4waySerialCommand(dst, src, mspPostProcessFn); + } else if (cmdMSP == MSP_SET_PASSTHROUGH) { + mspFcSetPassthroughCommand(dst, src, mspPostProcessFn); ret = MSP_RESULT_ACK; -#endif #ifdef USE_FLASHFS } else if (cmdMSP == MSP_DATAFLASH_READ) { mspFcDataFlashReadCommand(dst, src); diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index ef9978a7ac..1497d0b386 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -336,7 +336,7 @@ #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration #define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration -#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface +#define MSP_SET_PASSTHROUGH 245 //in message Sets up passthrough to different peripherals (4way interface, uart, etc...) #define MSP_SET_RTC 246 //in message Sets the RTC clock #define MSP_RTC 247 //out message Gets the RTC clock #define MSP_SET_BOARD_INFO 248 //in message Sets the board information for this board