rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Support per MSP source arming disable control

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
Show Stats 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;

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

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

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

@ -110,6 +110,7 @@ typedef struct mspPort_s {
uint8_t checksum1;
uint8_t checksum2;
bool sharedWithTelemetry;
mspDescriptor_t descriptor;
} mspPort_t;
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 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) {

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

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