Add support for emergency arming

Enabling and disabling the arming switch 10 times during a 10s
window will override checks for level, navigation unsafe, compass
not calibrated and some hardware failures (it will only enforce
gyro and acc to be working).

FLASH +224, RAM +24

Fixes #4489 and #4515

src/main/fc/fc_core.c

@@ -97,6 +97,21 @@ enum {
 
 #define GYRO_WATCHDOG_DELAY 100  // Watchdog for boards without interrupt for gyro
 
+#define EMERGENCY_ARMING_TIME_WINDOW_MS 10000
+#define EMERGENCY_ARMING_COUNTER_STEP_MS 100
+
+typedef struct emergencyArmingState_s {
+    bool armingSwitchWasOn;
+    // Each entry in the queue is an offset from start,
+    // in 0.1s increments. This lets us represent up to 25.5s
+    // so it will work fine as long as the window for the triggers
+    // is smaller (see EMERGENCY_ARMING_TIME_WINDOW_MS). First
+    // entry of the queue is implicit.
+    timeMs_t start;
+    uint8_t queue[9];
+    uint8_t queueCount;
+} emergencyArmingState_t;
+
 timeDelta_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
 static timeUs_t flightTime = 0;
 
@@ -108,6 +123,7 @@ uint8_t motorControlEnable = false;
 
 static bool isRXDataNew;
 static disarmReason_t lastDisarmReason = DISARM_NONE;
+static emergencyArmingState_t emergencyArming;
 
 bool isCalibrating(void)
 {
@@ -290,6 +306,41 @@ static void updateArmingStatus(void)
     }
 }
 
+static bool emergencyArmingIsTriggered(void)
+{
+    int threshold = (EMERGENCY_ARMING_TIME_WINDOW_MS / EMERGENCY_ARMING_COUNTER_STEP_MS);
+    return emergencyArming.queueCount == ARRAYLEN(emergencyArming.queue) + 1 &&
+        emergencyArming.queue[ARRAYLEN(emergencyArming.queue) - 1] < threshold &&
+        emergencyArming.start >= millis() - EMERGENCY_ARMING_TIME_WINDOW_MS;
+}
+
+static bool emergencyArmingCanOverrideArmingPrevention(void)
+{
+    uint32_t armingPrevention = armingFlags & ARMING_DISABLED_ALL_FLAGS;
+
+    // This flag is to require a switch flip if it's turned on while arming
+    // is disabled (otherwise craft could arm suddenly if the arming prevention
+    // reasons stop happening - e.g. THR high)
+    armingPrevention &= ~ARMING_DISABLED_ARM_SWITCH;
+
+    // Always override not level, unsafe navigation and compass not calibrated
+    armingPrevention &= ~ARMING_DISABLED_NOT_LEVEL;
+    armingPrevention &= ~ARMING_DISABLED_NAVIGATION_UNSAFE;
+    armingPrevention &= ~ARMING_DISABLED_COMPASS_NOT_CALIBRATED;
+
+    // In the event of a hardware failure, check wether the essential
+    // hardware is working. In that case, go ahead
+    if (getHwGyroStatus() == HW_SENSOR_OK && getHwAccelerometerStatus() == HW_SENSOR_OK) {
+        armingPrevention &= ~ARMING_DISABLED_HARDWARE_FAILURE;
+    }
+    return armingPrevention == 0;
+}
+
+static bool emergencyArmingIsEnabled(void)
+{
+    return emergencyArmingIsTriggered() && emergencyArmingCanOverrideArmingPrevention();
+}
+
 void annexCode(void)
 {
     int32_t throttleValue;
@@ -355,6 +406,39 @@ disarmReason_t getDisarmReason(void)
     return lastDisarmReason;
 }
 
+void emergencyArmingUpdate(bool armingSwitchIsOn)
+{
+    if (armingSwitchIsOn == emergencyArming.armingSwitchWasOn) {
+        return;
+    }
+    if (armingSwitchIsOn) {
+        timeMs_t now = millis();
+        if (emergencyArming.queueCount == 0) {
+            emergencyArming.queueCount = 1;
+            emergencyArming.start = now;
+        } else {
+            while (emergencyArming.start < now - EMERGENCY_ARMING_TIME_WINDOW_MS || emergencyArmingIsTriggered()) {
+                if (emergencyArming.queueCount > 1) {
+                    uint8_t delta = emergencyArming.queue[0];
+                    emergencyArming.start += delta * EMERGENCY_ARMING_COUNTER_STEP_MS;
+                    for (int ii = 0; ii < emergencyArming.queueCount - 2; ii++) {
+                        emergencyArming.queue[ii] = emergencyArming.queue[ii + 1] - delta;
+                    }
+                    emergencyArming.queueCount--;
+                } else {
+                    emergencyArming.start = now;
+                }
+            }
+            uint8_t delta = (now - emergencyArming.start) / EMERGENCY_ARMING_COUNTER_STEP_MS;
+            if (delta > 0) {
+                emergencyArming.queue[emergencyArming.queueCount - 1] = delta;
+                emergencyArming.queueCount++;
+            }
+        }
+    }
+    emergencyArming.armingSwitchWasOn = !emergencyArming.armingSwitchWasOn;
+}
+
 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
 
 void releaseSharedTelemetryPorts(void) {
@@ -369,7 +453,7 @@ void tryArm(void)
 {
     updateArmingStatus();
 
-    if (!isArmingDisabled()) {
+    if (!isArmingDisabled() || emergencyArmingIsEnabled()) {
         if (ARMING_FLAG(ARMED)) {
             return;
         }

src/main/fc/fc_core.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include <stdbool.h>
+
 #include "common/time.h"
 
 typedef enum disarmReason_e {
@@ -38,6 +40,8 @@ void disarm(disarmReason_t disarmReason);
 void tryArm(void);
 disarmReason_t getDisarmReason(void);
 
+void emergencyArmingUpdate(bool armingSwitchIsOn);
+
 bool isCalibrating(void);
 float getFlightTime(void);
 

src/main/fc/rc_controls.c

@@ -179,7 +179,9 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
     rcSticks = stTmp;
 
     // perform actions
-    if (IS_RC_MODE_ACTIVE(BOXARM)) {
+    bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM);
+    emergencyArmingUpdate(armingSwitchIsActive);
+    if (armingSwitchIsActive) {
         rcDisarmTimeMs = currentTimeMs;
         tryArm();
     } else {