diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index d867903a64..4f6a9118cb 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -332,7 +332,7 @@ void annexCode(void) updateArmingStatus(); } -void mwDisarm(disarmReason_t disarmReason) +void disarm(disarmReason_t disarmReason) { if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; @@ -365,7 +365,7 @@ void releaseSharedTelemetryPorts(void) { } } -void mwArm(void) +void tryArm(void) { updateArmingStatus(); @@ -427,7 +427,7 @@ void processRx(timeUs_t currentTimeUs) // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) - mwDisarm(DISARM_SWITCH_3D); + disarm(DISARM_SWITCH_3D); } updateRSSI(currentTimeUs); @@ -454,7 +454,7 @@ void processRx(timeUs_t currentTimeUs) && (int32_t)(disarmAt - millis()) < 0 ) { // auto-disarm configured and delay is over - mwDisarm(DISARM_TIMEOUT); + disarm(DISARM_TIMEOUT); armedBeeperOn = false; } else { // still armed; do warning beeps while armed @@ -485,7 +485,7 @@ void processRx(timeUs_t currentTimeUs) } } - processRcStickPositions(throttleStatus, armingConfig()->disarm_kill_switch, armingConfig()->fixed_wing_auto_arm); + processRcStickPositions(throttleStatus); updateActivatedModes(); diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index eccdf613bb..be1d29ae49 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -31,8 +31,8 @@ typedef enum disarmReason_e { void handleInflightCalibrationStickPosition(void); -void mwDisarm(disarmReason_t disarmReason); -void mwArm(void); +void disarm(disarmReason_t disarmReason); +void tryArm(void); disarmReason_t getDisarmReason(void); bool isCalibrating(void); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 105f0c20da..3aebc59a28 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -62,7 +62,8 @@ #include "sensors/acceleration.h" #define AIRMODE_DEADBAND 25 -#define MIN_RC_TICK_INTERVAL_MS 20 +#define MIN_RC_TICK_INTERVAL_MS 20 +#define DEFAULT_RC_SWITCH_DISARM_DELAY_MS 150 // Wait at least 150ms before disarming via switch stickPositions_e rcStickPositions; @@ -78,11 +79,13 @@ PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .deadband3d_throttle = 50 ); -PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 1); PG_RESET_TEMPLATE(armingConfig_t, armingConfig, + .fixed_wing_auto_arm = 0, .disarm_kill_switch = 1, - .auto_disarm_delay = 5 + .auto_disarm_delay = 5, + .switchDisarmDelayMs = DEFAULT_RC_SWITCH_DISARM_DELAY_MS, ); bool areSticksInApModePosition(uint16_t ap_mode) @@ -151,12 +154,12 @@ static void updateRcStickPositions(void) rcStickPositions = tmp; } -void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm) +void processRcStickPositions(throttleStatus_e throttleStatus) { static timeMs_t lastTickTimeMs = 0; static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint32_t rcSticks; // this hold sticks position for command combos - static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it + static timeMs_t rcDisarmTimeMs; // this is an extra guard for disarming through switch to prevent that one frame can disarm it const timeMs_t currentTimeMs = millis(); updateRcStickPositions(); @@ -177,29 +180,29 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s // perform actions if (!isUsingSticksForArming()) { if (IS_RC_MODE_ACTIVE(BOXARM)) { - rcDisarmTicks = 0; - mwArm(); + rcDisarmTimeMs = currentTimeMs; + tryArm(); } else { // Disarming via ARM BOX // Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver // and can't afford to risk disarming in the air if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) { - rcDisarmTicks++; - if (rcDisarmTicks > 3) { // Wait for at least 3 RX ticks (60ms @ 50Hz RX) - if (disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) { - mwDisarm(DISARM_SWITCH); + const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs; + if (disarmDelay > armingConfig()->switchDisarmDelayMs) { + if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) { + disarm(DISARM_SWITCH); } } } else { - rcDisarmTicks = 0; + rcDisarmTimeMs = currentTimeMs; } } } // KILLSWITCH disarms instantly if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) { - mwDisarm(DISARM_KILLSWITCH); + disarm(DISARM_KILLSWITCH); } if (rcDelayCommand != 20) { @@ -210,11 +213,11 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s // Disarm on throttle down + yaw if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { // Dont disarm if fixedwing and motorstop - if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) { + if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { return; } else if (ARMING_FLAG(ARMED)) { - mwDisarm(DISARM_STICKS); + disarm(DISARM_STICKS); } else { beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held @@ -274,17 +277,17 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s // Arming by sticks if (isUsingSticksForArming()) { - if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) { + if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { // Auto arm on throttle when using fixedwing and motorstop if (throttleStatus != THROTTLE_LOW) { - mwArm(); + tryArm(); return; } } else { if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) { // Arm via YAW - mwArm(); + tryArm(); return; } } diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 9aba71fafb..751bb51b2e 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -81,6 +81,7 @@ typedef struct armingConfig_s { uint8_t fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0 + uint16_t switchDisarmDelayMs; // additional delay between ARM box going off and actual disarm } armingConfig_t; PG_DECLARE(armingConfig_t, armingConfig); @@ -92,6 +93,6 @@ bool areSticksInApModePosition(uint16_t ap_mode); bool areSticksDeflectedMoreThanPosHoldDeadband(void); throttleStatus_e calculateThrottleStatus(void); rollPitchStatus_e calculateRollPitchCenterStatus(void); -void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm); +void processRcStickPositions(throttleStatus_e throttleStatus); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 50c7a86ba5..a34ebcc9e6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -710,6 +710,10 @@ groups: - name: auto_disarm_delay min: 0 max: 60 + - name: switch_disarm_delay + field: switchDisarmDelayMs + min: 0 + max: 1000 - name: PG_GPS_CONFIG type: gpsConfig_t diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 814badf898..b976ba564e 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -498,7 +498,7 @@ void failsafeUpdateState(void) case FAILSAFE_LANDED: ENABLE_ARMING_FLAG(ARMING_DISABLED_FAILSAFE_SYSTEM); // To prevent accidently rearming by an intermittent rx link - mwDisarm(DISARM_FAILSAFE); + disarm(DISARM_FAILSAFE); failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; failsafeState.controlling = false; // Failsafe no longer in control of the machine - release control to pilot diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index fc7cdd27a1..d95384a975 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1036,7 +1036,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigatio UNUSED(previousState); if (navConfig()->general.flags.disarm_on_landing) { - mwDisarm(DISARM_NAVIGATION); + disarm(DISARM_NAVIGATION); } return NAV_FSM_EVENT_SUCCESS;