diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9e6c0ddbf2..6c2ee8c419 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1754,6 +1754,9 @@ const clivalue_t valueTable[] = { { "displayport_msp_row_adjust", VAR_INT8 | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, rowAdjust) }, { "displayport_msp_fonts", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, fontSelection) }, { "displayport_msp_use_device_blink", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, useDeviceBlink) }, +#ifdef USE_MSP_DISPLAYPORT_DISARM_DELAY + { "displayport_msp_use_disarm_delay", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, useDisarmDelay) }, +#endif #endif // PG_DISPLAY_PORT_MSP_CONFIG diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 15cab1dec6..790d79be97 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -159,6 +159,10 @@ #include "hardware_revision.h" #endif +#ifdef USE_MSP_DISPLAYPORT_DISARM_DELAY +#include "io/displayport_msp.h" +#endif + #include "msp.h" static const char * const flightControllerIdentifier = FC_FIRMWARE_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller. @@ -1076,6 +1080,38 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce return true; } +#ifdef USE_MSP_DISPLAYPORT_DISARM_DELAY + +#define DISARM_DELAY_MULTIPLIER_US 100000 + +static void mspDisplayportDelayDisarm(mspDescriptor_t srcDesc, boxBitmask_t *flightModeFlags) +{ + static mspDescriptor_t displayPortMspDescriptor; + static bool displayPortMspArmState = false; + static bool descriptorInitialized = false; + + if (!descriptorInitialized) { + descriptorInitialized = true; + displayPortMspDescriptor = getMspSerialPortDescriptor(displayPortMspGetSerial()); + } + + if (displayPortMspDescriptor == srcDesc) { + bool currentState = bitArrayGet(flightModeFlags, BOXARM); + if (displayPortMspArmState) { + if (!currentState) { + if (cmpTimeUs(micros(), getLastDisarmTimeUs()) < DISARM_DELAY_MULTIPLIER_US * displayPortProfileMsp()->useDisarmDelay) { + bitArraySet(flightModeFlags, BOXARM); + } else { + displayPortMspArmState = false; + } + } + } else { + displayPortMspArmState = currentState; + } + } +} +#endif + static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t *dst) { bool unsupportedCommand = false; @@ -1089,6 +1125,9 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t case MSP_STATUS: { boxBitmask_t flightModeFlags; const int flagBits = packFlightModeFlags(&flightModeFlags); +#ifdef USE_MSP_DISPLAYPORT_DISARM_DELAY + mspDisplayportDelayDisarm(srcDesc, &flightModeFlags); +#endif sbufWriteU16(dst, getTaskDeltaTimeUs(TASK_PID)); #ifdef USE_I2C diff --git a/src/main/pg/displayport_profiles.c b/src/main/pg/displayport_profiles.c index 3d275d25ef..03cc9c1e34 100644 --- a/src/main/pg/displayport_profiles.c +++ b/src/main/pg/displayport_profiles.c @@ -37,6 +37,9 @@ void pgResetFn_displayPortProfileMsp(displayPortProfile_t *displayPortProfile) for (uint8_t font = 0; font < DISPLAYPORT_SEVERITY_COUNT; font++) { displayPortProfile->fontSelection[font] = font; } +#ifdef USE_MSP_DISPLAYPORT_DISARM_DELAY + displayPortProfile->useDisarmDelay = 30; +#endif } #endif diff --git a/src/main/pg/displayport_profiles.h b/src/main/pg/displayport_profiles.h index 22a7968c00..fee08f355b 100644 --- a/src/main/pg/displayport_profiles.h +++ b/src/main/pg/displayport_profiles.h @@ -34,6 +34,9 @@ typedef struct displayPortProfile_s { uint8_t fontSelection[DISPLAYPORT_SEVERITY_COUNT]; uint8_t useDeviceBlink; // Use device local blink capability +#ifdef USE_MSP_DISPLAYPORT_DISARM_DELAY + uint8_t useDisarmDelay; +#endif } displayPortProfile_t; PG_DECLARE(displayPortProfile_t, displayPortProfileMsp);