Skip to content

Commit

Permalink
Support per MSP source arming disable control
Browse files Browse the repository at this point in the history
  • Loading branch information
@ezshinoda
ezshinoda committed Oct 16, 2019
1 parent 6ff2022 commit fb2ec7f
Show file tree
Hide file tree
Showing 6 changed files with 56 additions and 16 deletions.
50 changes: 39 additions & 11 deletions src/main/msp/msp.c
View file
Expand Up @@ -197,6 +197,30 @@ static uint32_t getFeatureMask(void)
}
}

static int mspDescriptor = 0;

mspDescriptor_t mspDescriptorAlloc(void)
{
return (mspDescriptor_t)mspDescriptor++;
}

static uint32_t mspArmingDisableFlags = 0;

static void mspArmingDisableByDescriptor(mspDescriptor_t desc)
{
mspArmingDisableFlags |= (1 << desc);
}

static void mspArmingEnableByDescriptor(mspDescriptor_t desc)
{
mspArmingDisableFlags &= ~(1 << desc);
}

static bool mspIsMspArmingEnabled(void)
{
return mspArmingDisableFlags == 0;
}

#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#define ESC_4WAY 0xff

Expand Down Expand Up @@ -1748,7 +1772,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
return !unsupportedCommand;
}

static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
static mspResult_e mspFcProcessOutCommandWithArg(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src, sbuf_t *dst, mspPostProcessFnPtr *mspPostProcessFn)
{

switch (cmdMSP) {
Expand Down Expand Up @@ -1812,7 +1836,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sb
uint8_t newMSP = sbufReadU8(src);
sbufInit(&packetOut.buf, dst->ptr, dst->end);
packetIn.cmd = newMSP;
mspFcProcessCommand(&packetIn, &packetOut, NULL);
mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
uint8_t mspSize = sbufPtr(&packetOut.buf) - dst->ptr;
mspSize++; // need to add length information for each MSP
bytesRemaining -= mspSize;
Expand All @@ -1826,7 +1850,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sb
uint8_t* sizePtr = sbufPtr(&packetOut.buf);
sbufWriteU8(&packetOut.buf, 0); // dummy
packetIn.cmd = sbufReadU8(src);
mspFcProcessCommand(&packetIn, &packetOut, NULL);
mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
(*sizePtr) = sbufPtr(&packetOut.buf) - (sizePtr + 1);
}
dst->ptr = packetOut.buf.ptr;
Expand Down Expand Up @@ -1934,7 +1958,7 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
}
#endif

static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src)
{
uint32_t i;
uint8_t value;
Expand Down Expand Up @@ -2731,6 +2755,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
disableRunawayTakeoff = sbufReadU8(src);
}
if (command) {
mspArmingDisableByDescriptor(srcDesc);
setArmingDisabled(ARMING_DISABLED_MSP);
if (ARMING_FLAG(ARMED)) {
disarm();
Expand All @@ -2739,10 +2764,13 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
runawayTakeoffTemporaryDisable(false);
#endif
} else {
unsetArmingDisabled(ARMING_DISABLED_MSP);
mspArmingEnableByDescriptor(srcDesc);
if (mspIsMspArmingEnabled()) {
unsetArmingDisabled(ARMING_DISABLED_MSP);
#ifdef USE_RUNAWAY_TAKEOFF
runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
#endif
}
}
}
break;
Expand Down Expand Up @@ -3049,7 +3077,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ACK;
}

static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
static mspResult_e mspCommonProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
{
UNUSED(mspPostProcessFn);
const unsigned int dataSize = sbufBytesRemaining(src);
Expand Down Expand Up @@ -3262,15 +3290,15 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPos
#endif // OSD

default:
return mspProcessInCommand(cmdMSP, src);
return mspProcessInCommand(srcDesc, cmdMSP, src);
}
return MSP_RESULT_ACK;
}

/*
* Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
*/
mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
{
int ret = MSP_RESULT_ACK;
sbuf_t *dst = &reply->buf;
Expand All @@ -3283,7 +3311,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
ret = MSP_RESULT_ACK;
} else if (mspProcessOutCommand(cmdMSP, dst)) {
ret = MSP_RESULT_ACK;
} else if ((ret = mspFcProcessOutCommandWithArg(cmdMSP, src, dst, mspPostProcessFn)) != MSP_RESULT_CMD_UNKNOWN) {
} 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) {
Expand All @@ -3296,7 +3324,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
ret = MSP_RESULT_ACK;
#endif
} else {
ret = mspCommonProcessInCommand(cmdMSP, src, mspPostProcessFn);
ret = mspCommonProcessInCommand(srcDesc, cmdMSP, src, mspPostProcessFn);
}
reply->result = ret;
return ret;
Expand Down
8 changes: 6 additions & 2 deletions src/main/msp/msp.h
View file
Expand Up @@ -54,12 +54,16 @@ typedef struct mspPacket_s {
uint8_t direction;
} mspPacket_t;

typedef int mspDescriptor_t;

struct serialPort_s;
typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc.
typedef mspResult_e (*mspProcessCommandFnPtr)(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
typedef mspResult_e (*mspProcessCommandFnPtr)(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
typedef void (*mspProcessReplyFnPtr)(mspPacket_t *cmd);


void mspInit(void);
mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
void mspFcProcessReply(mspPacket_t *reply);

mspDescriptor_t mspDescriptorAlloc(void);
3 changes: 2 additions & 1 deletion src/main/msp/msp_serial.c
View file
Expand Up @@ -48,6 +48,7 @@ static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, bo

mspPortToReset->port = serialPort;
mspPortToReset->sharedWithTelemetry = sharedWithTelemetry;
mspPortToReset->descriptor = mspDescriptorAlloc();
}

void mspSerialAllocatePorts(void)
Expand Down Expand Up @@ -413,7 +414,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
};

mspPostProcessFnPtr mspPostProcessFn = NULL;
const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
const mspResult_e status = mspProcessCommandFn(msp->descriptor, &command, &reply, &mspPostProcessFn);

if (status != MSP_RESULT_NO_REPLY) {
sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
Expand Down
1 change: 1 addition & 0 deletions src/main/msp/msp_serial.h
View file
Expand Up @@ -110,6 +110,7 @@ typedef struct mspPort_s {
uint8_t checksum1;
uint8_t checksum2;
bool sharedWithTelemetry;
mspDescriptor_t descriptor;
} mspPort_t;

void mspSerialInit(void);
Expand Down
5 changes: 4 additions & 1 deletion src/main/telemetry/msp_shared.c
View file
Expand Up @@ -59,6 +59,7 @@ static mspRxBuffer_t mspRxBuffer;
static mspTxBuffer_t mspTxBuffer;
static mspPacket_t mspRxPacket;
static mspPacket_t mspTxPacket;
static mspDescriptor_t mspSharedDescriptor;

void initSharedMsp(void)
{
Expand All @@ -71,6 +72,8 @@ void initSharedMsp(void)
mspPackage.responsePacket = &mspTxPacket;
mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;

mspSharedDescriptor = mspDescriptorAlloc();
}

static void processMspPacket(void)
Expand All @@ -80,7 +83,7 @@ static void processMspPacket(void)
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;

mspPostProcessFnPtr mspPostProcessFn = NULL;
if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
if (mspFcProcessCommand(mspSharedDescriptor, mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
}
if (mspPostProcessFn) {
Expand Down
5 changes: 4 additions & 1 deletion src/test/unit/telemetry_crsf_msp_unittest.cc
View file
Expand Up @@ -284,8 +284,11 @@ extern "C" {

bool airmodeIsEnabled(void) {return true;}

mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
mspDescriptor_t mspDescriptorAlloc(void) {return 0;}

mspResult_e mspFcProcessCommand(mspDescriptor_t srcDesc, mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {

UNUSED(srcDesc);
UNUSED(mspPostProcessFn);

sbuf_t *dst = &reply->buf;
Expand Down

0 comments on commit fb2ec7f

Please sign in to comment.