Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
digitalentity merged 1 commit into from
Jan 30, 2020
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
78 changes: 61 additions & 17 deletions src/main/fc/fc_msp.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,

MSP_PASSTHROUGH_ESC_4WAY = 0xFF,
} mspPassthroughType_e;

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 uint8_t escMode;
static uint8_t escPortIndex;
static void mspSerialPassthroughFn(serialPort_t *serialPort)
{
serialPort_t *passthroughPort = mspFindPassthroughSerialPort();
if (passthroughPort && serialPort) {
serialPassthrough(passthroughPort, serialPort, NULL, NULL);
}
}

static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
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
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion src/main/msp/msp_protocol.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -321,7 +321,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_RTC 246 //out message Gets the RTC clock (returns: secs(i32) millis(u16) - (0,0) if time is not known)
#define MSP_SET_RTC 247 //in message Sets the RTC clock (args: secs(i32) millis(u16))

Expand Down