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

Support per MSP source arming disable control

Browse source
This commit is contained in:
jflyper 2019-10-14 19:12:42 +09:00
parent 6ff2022a2a
commit fb2ec7fe68
6 changed files with 56 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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