diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a2c6362fc4..8beac13f17 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -140,6 +140,7 @@ static uint8_t cliWriteBuffer[sizeof(*cliWriter) + 128]; static char cliBuffer[64]; static uint32_t bufferIndex = 0; +static uint16_t cliDelayMs = 0; #if defined(USE_ASSERT) static void cliAssert(char *cmdline); @@ -222,6 +223,9 @@ static void cliPrint(const char *str) static void cliPrintLinefeed(void) { cliPrint("\r\n"); + if (cliDelayMs) { + delay(cliDelayMs); + } } static void cliPrintLine(const char *str) @@ -1672,6 +1676,25 @@ static void cliModeColor(char *cmdline) } #endif +static void cliDelay(char* cmdLine) { + int ms = 0; + if (isEmpty(cmdLine)) { + cliDelayMs = 0; + cliPrintLine("CLI delay deactivated"); + return; + } + + ms = fastA2I(cmdLine); + if (ms) { + cliDelayMs = ms; + cliPrintLinef("CLI delay set to %d ms", ms); + + } else { + cliShowParseError(); + } + +} + static void printServo(uint8_t dumpMask, const servoParam_t *servoParam, const servoParam_t *defaultServoParam) { // print out servo settings @@ -3856,6 +3879,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), CLI_COMMAND_DEF("mode_color", "configure mode and special colors", NULL, cliModeColor), #endif + CLI_COMMAND_DEF("cli_delay", "CLI Delay", "Delay in ms", cliDelay), CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), CLI_COMMAND_DEF("diff", "list configuration changes from default", diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d5dbc274a7..8e5a07e24f 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -534,18 +534,6 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; #ifdef USE_PROGRAMMING_FRAMEWORK - case MSP2_INAV_LOGIC_CONDITIONS: - for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { - sbufWriteU8(dst, logicConditions(i)->enabled); - sbufWriteU8(dst, logicConditions(i)->activatorId); - sbufWriteU8(dst, logicConditions(i)->operation); - sbufWriteU8(dst, logicConditions(i)->operandA.type); - sbufWriteU32(dst, logicConditions(i)->operandA.value); - sbufWriteU8(dst, logicConditions(i)->operandB.type); - sbufWriteU32(dst, logicConditions(i)->operandB.value); - sbufWriteU8(dst, logicConditions(i)->flags); - } - break; case MSP2_INAV_LOGIC_CONDITIONS_STATUS: for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { sbufWriteU32(dst, logicConditionGetValue(i)); @@ -1547,6 +1535,23 @@ static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src) } } +static mspResult_e mspFcLogicConditionCommand(sbuf_t *dst, sbuf_t *src) { + const uint8_t idx = sbufReadU8(src); + if (idx < MAX_LOGIC_CONDITIONS) { + sbufWriteU8(dst, logicConditions(idx)->enabled); + sbufWriteU8(dst, logicConditions(idx)->activatorId); + sbufWriteU8(dst, logicConditions(idx)->operation); + sbufWriteU8(dst, logicConditions(idx)->operandA.type); + sbufWriteU32(dst, logicConditions(idx)->operandA.value); + sbufWriteU8(dst, logicConditions(idx)->operandB.type); + sbufWriteU32(dst, logicConditions(idx)->operandB.value); + sbufWriteU8(dst, logicConditions(idx)->flags); + return MSP_RESULT_ACK; + } else { + return MSP_RESULT_ERROR; + } +} + static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src) { const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number @@ -3225,6 +3230,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu break; #endif +#ifdef USE_PROGRAMMING_FRAMEWORK + case MSP2_INAV_LOGIC_CONDITIONS: + *ret = mspFcLogicConditionCommand(dst, src); + break; +#endif case MSP2_INAV_SAFEHOME: *ret = mspFcSafeHomeOutCommand(dst, src); break;