From fb2ec7fe68898fa693f9a63cdb7a229e915adf15 Mon Sep 17 00:00:00 2001 From: jflyper Date: Mon, 14 Oct 2019 19:12:42 +0900 Subject: [PATCH] Support per MSP source arming disable control --- src/main/msp/msp.c | 50 +++++++++++++++----- src/main/msp/msp.h | 8 +++- src/main/msp/msp_serial.c | 3 +- src/main/msp/msp_serial.h | 1 + src/main/telemetry/msp_shared.c | 5 +- src/test/unit/telemetry_crsf_msp_unittest.cc | 5 +- 6 files changed, 56 insertions(+), 16 deletions(-) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index f627e45bdae..c027008976e 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -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 @@ -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) { @@ -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; @@ -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; @@ -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; @@ -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(); @@ -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; @@ -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); @@ -3262,7 +3290,7 @@ 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; } @@ -3270,7 +3298,7 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPos /* * 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; @@ -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) { @@ -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; diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index a1dc890d561..cc19c8767d4 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -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); diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index dbdb4e223ec..ae38ea91776 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -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) @@ -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 diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index d4adb04e520..010f489451d 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -110,6 +110,7 @@ typedef struct mspPort_s { uint8_t checksum1; uint8_t checksum2; bool sharedWithTelemetry; + mspDescriptor_t descriptor; } mspPort_t; void mspSerialInit(void); diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 06c3086be54..8b45979855c 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -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) { @@ -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) @@ -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) { diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 3591916b8de..ce021e235f8 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -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;