Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 14:55:21 +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

46
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 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
#define ESC_4WAY 0xff #define ESC_4WAY 0xff
@ -1748,7 +1772,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
return !unsupportedCommand; 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) { switch (cmdMSP) {
@ -1812,7 +1836,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sb
uint8_t newMSP = sbufReadU8(src); uint8_t newMSP = sbufReadU8(src);
sbufInit(&packetOut.buf, dst->ptr, dst->end); sbufInit(&packetOut.buf, dst->ptr, dst->end);
packetIn.cmd = newMSP; packetIn.cmd = newMSP;
mspFcProcessCommand(&packetIn, &packetOut, NULL); mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
uint8_t mspSize = sbufPtr(&packetOut.buf) - dst->ptr; uint8_t mspSize = sbufPtr(&packetOut.buf) - dst->ptr;
mspSize++; // need to add length information for each MSP mspSize++; // need to add length information for each MSP
bytesRemaining -= mspSize; bytesRemaining -= mspSize;
@ -1826,7 +1850,7 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *src, sb
uint8_t* sizePtr = sbufPtr(&packetOut.buf); uint8_t* sizePtr = sbufPtr(&packetOut.buf);
sbufWriteU8(&packetOut.buf, 0); // dummy sbufWriteU8(&packetOut.buf, 0); // dummy
packetIn.cmd = sbufReadU8(src); packetIn.cmd = sbufReadU8(src);
mspFcProcessCommand(&packetIn, &packetOut, NULL); mspFcProcessCommand(srcDesc, &packetIn, &packetOut, NULL);
(*sizePtr) = sbufPtr(&packetOut.buf) - (sizePtr + 1); (*sizePtr) = sbufPtr(&packetOut.buf) - (sizePtr + 1);
} }
dst->ptr = packetOut.buf.ptr; dst->ptr = packetOut.buf.ptr;
@ -1934,7 +1958,7 @@ static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
} }
#endif #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; uint32_t i;
uint8_t value; uint8_t value;
@ -2731,6 +2755,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
disableRunawayTakeoff = sbufReadU8(src); disableRunawayTakeoff = sbufReadU8(src);
} }
if (command) { if (command) {
mspArmingDisableByDescriptor(srcDesc);
setArmingDisabled(ARMING_DISABLED_MSP); setArmingDisabled(ARMING_DISABLED_MSP);
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
disarm(); disarm();
@ -2739,12 +2764,15 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
runawayTakeoffTemporaryDisable(false); runawayTakeoffTemporaryDisable(false);
#endif #endif
} else { } else {
mspArmingEnableByDescriptor(srcDesc);
if (mspIsMspArmingEnabled()) {
unsetArmingDisabled(ARMING_DISABLED_MSP); unsetArmingDisabled(ARMING_DISABLED_MSP);
#ifdef USE_RUNAWAY_TAKEOFF #ifdef USE_RUNAWAY_TAKEOFF
runawayTakeoffTemporaryDisable(disableRunawayTakeoff); runawayTakeoffTemporaryDisable(disableRunawayTakeoff);
#endif #endif
} }
} }
}
break; break;
#ifdef USE_FLASHFS #ifdef USE_FLASHFS
@ -3049,7 +3077,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ACK; 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); UNUSED(mspPostProcessFn);
const unsigned int dataSize = sbufBytesRemaining(src); const unsigned int dataSize = sbufBytesRemaining(src);
@ -3262,7 +3290,7 @@ static mspResult_e mspCommonProcessInCommand(uint8_t cmdMSP, sbuf_t *src, mspPos
#endif // OSD #endif // OSD
default: default:
return mspProcessInCommand(cmdMSP, src); return mspProcessInCommand(srcDesc, cmdMSP, src);
} }
return MSP_RESULT_ACK; 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 * 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; int ret = MSP_RESULT_ACK;
sbuf_t *dst = &reply->buf; sbuf_t *dst = &reply->buf;
@ -3283,7 +3311,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
ret = MSP_RESULT_ACK; ret = MSP_RESULT_ACK;
} else if (mspProcessOutCommand(cmdMSP, dst)) { } else if (mspProcessOutCommand(cmdMSP, dst)) {
ret = MSP_RESULT_ACK; 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 */; /* ret */;
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
} else if (cmdMSP == MSP_SET_4WAY_IF) { } 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; ret = MSP_RESULT_ACK;
#endif #endif
} else { } else {
ret = mspCommonProcessInCommand(cmdMSP, src, mspPostProcessFn); ret = mspCommonProcessInCommand(srcDesc, cmdMSP, src, mspPostProcessFn);
} }
reply->result = ret; reply->result = ret;
return ret; return ret;

8
src/main/msp/msp.h
View file

@ -54,12 +54,16 @@ typedef struct mspPacket_s {
uint8_t direction; uint8_t direction;
} mspPacket_t; } mspPacket_t;
typedef int mspDescriptor_t;
struct serialPort_s; struct serialPort_s;
typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc. 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); typedef void (*mspProcessReplyFnPtr)(mspPacket_t *cmd);
void mspInit(void); 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); void mspFcProcessReply(mspPacket_t *reply);
mspDescriptor_t mspDescriptorAlloc(void);

3
src/main/msp/msp_serial.c
View file

@ -48,6 +48,7 @@ static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, bo
mspPortToReset->port = serialPort; mspPortToReset->port = serialPort;
mspPortToReset->sharedWithTelemetry = sharedWithTelemetry; mspPortToReset->sharedWithTelemetry = sharedWithTelemetry;
mspPortToReset->descriptor = mspDescriptorAlloc();
} }
void mspSerialAllocatePorts(void) void mspSerialAllocatePorts(void)
@ -413,7 +414,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
}; };
mspPostProcessFnPtr mspPostProcessFn = NULL; 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) { if (status != MSP_RESULT_NO_REPLY) {
sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction

1
src/main/msp/msp_serial.h
View file

@ -110,6 +110,7 @@ typedef struct mspPort_s {
uint8_t checksum1; uint8_t checksum1;
uint8_t checksum2; uint8_t checksum2;
bool sharedWithTelemetry; bool sharedWithTelemetry;
mspDescriptor_t descriptor;
} mspPort_t; } mspPort_t;
void mspSerialInit(void); void mspSerialInit(void);

5
src/main/telemetry/msp_shared.c
View file

@ -59,6 +59,7 @@ static mspRxBuffer_t mspRxBuffer;
static mspTxBuffer_t mspTxBuffer; static mspTxBuffer_t mspTxBuffer;
static mspPacket_t mspRxPacket; static mspPacket_t mspRxPacket;
static mspPacket_t mspTxPacket; static mspPacket_t mspTxPacket;
static mspDescriptor_t mspSharedDescriptor;
void initSharedMsp(void) void initSharedMsp(void)
{ {
@ -71,6 +72,8 @@ void initSharedMsp(void)
mspPackage.responsePacket = &mspTxPacket; mspPackage.responsePacket = &mspTxPacket;
mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer; mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
mspSharedDescriptor = mspDescriptorAlloc();
} }
static void processMspPacket(void) static void processMspPacket(void)
@ -80,7 +83,7 @@ static void processMspPacket(void)
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
mspPostProcessFnPtr mspPostProcessFn = NULL; 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); sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
} }
if (mspPostProcessFn) { if (mspPostProcessFn) {

5
src/test/unit/telemetry_crsf_msp_unittest.cc
View file

@ -284,8 +284,11 @@ extern "C" {
bool airmodeIsEnabled(void) {return true;} 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); UNUSED(mspPostProcessFn);
sbuf_t *dst = &reply->buf; sbuf_t *dst = &reply->buf;