Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

[MSP] Add support for setting up serial passthrough via MSP

Browse source 
Change MSP_SET_4WAY_IF to MSP_SET_PASSTHROUGH while also keeping
backwards compatibility.
This commit is contained in:
Alberto García Hierro 2020-01-28 21:32:25 +00:00
parent b9f41e6fd0
commit b6d143c055
2 changed files with 62 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

78
src/main/fc/fc_msp.c
View file

@ -153,26 +153,74 @@ typedef enum {
MSP_FLASHFS_BIT_SUPPORTED = 2
} mspFlashfsFlags_e;
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#define ESC_4WAY 0xff
typedef enum {
MSP_PASSTHROUGH_SERIAL_ID = 0xFD,
MSP_PASSTHROUGH_SERIAL_FUNCTION_ID = 0xFE,
static uint8_t escMode;
static uint8_t escPortIndex;
MSP_PASSTHROUGH_ESC_4WAY = 0xFF,
} mspPassthroughType_e;
static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
static uint8_t mspPassthroughMode;
static uint8_t mspPassthroughArgument;
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);
if (!sbufReadU8Safe(&mspPassthroughArgument, src)) {
mspPassthroughArgument = 0;
}
}
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
@ -182,14 +230,12 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr
*mspPostProcessFn = esc4wayProcess;
}
break;
#endif
default:
sbufWriteU8(dst, 0);
}
}
#endif
static void mspRebootFn(serialPort_t *serialPort)
{
UNUSED(serialPort);
@ -3177,11 +3223,9 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
ret = mspProcessSensorCommand(cmdMSP, src);
} else if (mspFcProcessOutCommand(cmdMSP, dst, mspPostProcessFn)) {
ret = MSP_RESULT_ACK;
#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
} else {
if (!mspFCProcessInOutCommand(cmdMSP, dst, src, &ret)) {
ret = mspFcProcessInCommand(cmdMSP, src);