Merge pull request #10099 from breadoven/abo_mr_inverted_crash_detection

Multirotor inverted crash detection

docs/Settings.md

@@ -3742,6 +3742,16 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
 
 ---
 
+### nav_mc_inverted_crash_detection
+
+Setting a value > 0 enables inverted crash detection for multirotors. It will auto disarm in situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 0 | 0 | 15 |
+
+---
+
 ### nav_mc_manual_climb_rate
 
 Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]

src/main/fc/settings.yaml

@@ -2470,6 +2470,12 @@ groups:
         default_value: OFF
         field: general.flags.landing_bump_detection
         type: bool
+      - name: nav_mc_inverted_crash_detection
+        description: "Setting a value > 0 enables inverted crash detection for multirotors. It will auto disarm in situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control or for some other reason. When enabled this setting defines the additional number of seconds before disarm beyond a minimum fixed time delay of 3s. Requires a barometer to work."
+        default_value: 0
+        field: mc.inverted_crash_detection
+        min: 0
+        max: 15
       - name: nav_mc_althold_throttle
         description: "If set to STICK the FC remembers the throttle stick position when enabling ALTHOLD and treats it as the neutral midpoint for holding altitude. If set to MID_STICK or HOVER the neutral midpoint is set to the mid stick position or the hover throttle position respectively."
         default_value: "STICK"

src/main/navigation/navigation.c

@@ -200,6 +200,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT,                                      // posResponseExpo * 100
         .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT,
         .althold_throttle_type = SETTING_NAV_MC_ALTHOLD_THROTTLE_DEFAULT,                        // STICK
+        .inverted_crash_detection = SETTING_NAV_MC_INVERTED_CRASH_DETECTION_DEFAULT,             // 0 - disarm time delay for inverted crash detection
     },
 
     // Fixed wing

src/main/navigation/navigation.h

@@ -344,6 +344,7 @@ typedef struct navConfig_s {
         uint8_t posResponseExpo;                // Position controller expo (taret vel expo for MC)
         bool slowDownForTurning;                // Slow down during WP missions when changing heading on next waypoint
         uint8_t althold_throttle_type;          // throttle zero datum type for alt hold
+        uint8_t inverted_crash_detection;       // Enables inverted crash detection, setting defines disarm time delay (0 = disabled)
     } mc;
 
     struct {
@@ -688,7 +689,7 @@ float getEstimatedAglPosition(void);
 bool isEstimatedAglTrusted(void);
 
 void checkManualEmergencyLandingControl(bool forcedActivation);
-float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue);
+void updateBaroAltitudeRate(float newBaroAltRate);
 bool rthAltControlStickOverrideCheck(uint8_t axis);
 
 int8_t navCheckActiveAngleHoldAxis(void);

src/main/navigation/navigation_multicopter.c

@@ -36,6 +36,7 @@
 #include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
 
+#include "fc/fc_core.h"
 #include "fc/config.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_curves.h"
@@ -757,14 +758,11 @@ bool isMulticopterFlying(void)
  * Multicopter land detector
  *-----------------------------------------------------------*/
 #if defined(USE_BARO)
-float updateBaroAltitudeRate(float newBaroAltRate, bool updateValue)
-{
 static float baroAltRate;
-    if (updateValue) {
-        baroAltRate = newBaroAltRate;
-    }
 
-    return baroAltRate;
+void updateBaroAltitudeRate(float newBaroAltRate)
+{
+    baroAltRate = newBaroAltRate;
 }
 
 static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
@@ -775,7 +773,6 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
      * Throttle trigger: must be below hover throttle with lower threshold for manual throttle control */
 
     static timeMs_t gSpikeDetectTimeMs = 0;
-    float baroAltRate = updateBaroAltitudeRate(0, false);
 
     if (!gSpikeDetectTimeMs && acc.accADCf[Z] > 2.0f && baroAltRate < 0.0f) {
         gSpikeDetectTimeMs = currentTimeMs;
@@ -793,7 +790,28 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
 
     return false;
 }
+
+bool isMulticopterCrashedInverted(timeMs_t currentTimeMs)
+{
+    /* Disarms MR if inverted on the ground. Checks vertical velocity is low based on Baro rate below 2 m/s */
+
+    static timeMs_t startTime = 0;
+
+    if ((ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) && fabsf(baroAltRate) < 200.0f) {
+        if (startTime == 0) {
+            startTime = currentTimeMs;
+        }
+
+        /* Minimum 3s disarm delay + extra user set delay time (min overall delay of 4s) */
+        uint16_t disarmTimeDelay = 3000 + S2MS(navConfig()->mc.inverted_crash_detection);
+        return currentTimeMs - startTime > disarmTimeDelay;
+    }
+
+    startTime = 0;
+    return false;
+}
 #endif
+
 bool isMulticopterLandingDetected(void)
 {
     DEBUG_SET(DEBUG_LANDING, 4, 0);
@@ -802,14 +820,23 @@ bool isMulticopterLandingDetected(void)
     const timeMs_t currentTimeMs = millis();
 
 #if defined(USE_BARO)
-    /* G bump landing detection only used when xy velocity is usable and low or failsafe is active */
-    bool gBumpDetectionUsable = navConfig()->general.flags.landing_bump_detection && sensors(SENSOR_BARO) &&
+    if (sensors(SENSOR_BARO)) {
+        /* Inverted crash landing detection - immediate disarm */
+        if (navConfig()->mc.inverted_crash_detection && !FLIGHT_MODE(TURTLE_MODE) && isMulticopterCrashedInverted(currentTimeMs)) {
+            ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
+            disarm(DISARM_LANDING);
+        }
+
+        /* G bump landing detection *
+         * Only used when xy velocity is low or failsafe is active */
+        bool gBumpDetectionUsable = navConfig()->general.flags.landing_bump_detection &&
                                     ((posControl.flags.estPosStatus >= EST_USABLE && posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING) ||
                                     FLIGHT_MODE(FAILSAFE_MODE));
 
         if (gBumpDetectionUsable && isLandingGbumpDetected(currentTimeMs)) {
             return true;    // Landing flagged immediately if landing bump detected
         }
+    }
 #endif
 
     bool throttleIsBelowMidHover = rcCommand[THROTTLE] < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));

src/main/navigation/navigation_pos_estimator.c

@@ -299,7 +299,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
             static float baroAltPrevious = 0;
             posEstimator.baro.baroAltRate = (posEstimator.baro.alt - baroAltPrevious) / US2S(baroDtUs);
             baroAltPrevious = posEstimator.baro.alt;
-            updateBaroAltitudeRate(posEstimator.baro.baroAltRate, true);
+            updateBaroAltitudeRate(posEstimator.baro.baroAltRate);
         }
     }
     else {