Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 23:05:19 +03:00
Code Issues Wiki Activity

Add support for serial passthrough via MSP

Browse source 
- 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.
This commit is contained in:
Alberto García Hierro 2019-12-08 22:48:57 +00:00
parent 18c485707e
commit deeabdbca3
2 changed files with 76 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

99
src/main/msp/msp.c
View file

@ -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);

2
src/main/msp/msp_protocol.h
View file

@ -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