CR128

breadoven · Jun 2, 2024 · 8a72173 · 8a72173
1 parent eaf4e2f
commit 8a72173
Show file tree

Hide file tree

Showing 5 changed files with 40 additions and 3 deletions.
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -2422,6 +2422,14 @@ groups:
         default_value: OFF
         field: general.flags.landing_bump_detection
         type: bool
+        # CR128
+      - name: nav_inverted_crash_detection
+        description: "Setting a value > 0 enables inverted crash detection for multirotors with the set value being the number of seconds before the multirotor is auto disarmed. It is intended for situations where the multirotor has crashed inverted on the ground and can't be manually disarmed due to loss of control. Detection has a minimum time delay before disarm of 3s when enabled."
+        default_value: 0
+        field: general.inverted_crash_detection
+        min: 0
+        max: 15
+        # CR128
       - 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"

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -174,8 +174,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT,                 // Changes sensitivity of landing detection
         .auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT,                             // 2000 ms - time delay to disarm when auto disarm after landing enabled
         .rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
-        .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT,                                // 20dps
+        .cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT,                                 // 20dps
         .rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT,                       // Delay before landing in FS. 0 = immedate landing
+        .inverted_crash_detection = SETTING_NAV_INVERTED_CRASH_DETECTION_DEFAULT,  // CR128     // 0 - sets disarm time delay for inverted crash detection
     },
 
     // MC-specific

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -324,6 +324,7 @@ typedef struct navConfig_s {
         uint16_t rth_linear_descent_start_distance; // Distance from home to start the linear descent (0 = immediately)
         uint8_t  cruise_yaw_rate;                   // Max yaw rate (dps) when CRUISE MODE is enabled
         uint16_t rth_fs_landing_delay;              // Delay upon reaching home before starting landing if in FS (0 = immediate)
+        uint8_t inverted_crash_detection;           //  // CR128
     } general;
 
     struct {

diff --git a/src/main/navigation/navigation_multicopter.c b/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"
@@ -804,13 +805,39 @@ static bool isLandingGbumpDetected(timeMs_t currentTimeMs)
     return false;
 }
 #endif
+// CR128
+bool isMulticopterCrashedInverted(void)
+{
+    static timeMs_t startTime = 0;
+
+    if ((ABS(attitude.values.roll) > 1000 || ABS(attitude.values.roll) > 700) && fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < MC_LAND_CHECK_VEL_Z_MOVING) {
+        if (startTime == 0) {
+            startTime = millis();
+        } else {
+            uint16_t disarmTimeDelay = S2MS(navConfig()->general.inverted_crash_detection);
+            disarmTimeDelay = disarmTimeDelay < 3000 ? 3000 : disarmTimeDelay;
+
+            return millis() - startTime > disarmTimeDelay;
+        }
+    } else {
+        startTime = 0;
+    }
+
+    return false;
+}
+// CR128
 bool isMulticopterLandingDetected(void)
 {
     DEBUG_SET(DEBUG_LANDING, 4, 0);
     DEBUG_SET(DEBUG_LANDING, 3, averageAbsGyroRates() * 100);
 
     const timeMs_t currentTimeMs = millis();
-
+// CR128
+    if (navConfig()->general.inverted_crash_detection && isMulticopterCrashedInverted()) {
+        ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
+        disarm(DISARM_LANDING);
+    }
+// CR128
 #if defined(USE_BARO)
     if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection && isLandingGbumpDetected(currentTimeMs)) {
         return true;    // Landing flagged immediately if landing bump detected

diff --git a/src/main/target/common.h b/src/main/target/common.h
@@ -179,7 +179,7 @@
 // Wind estimator
 #define USE_WIND_ESTIMATOR
 
-#define USE_SIMULATOR
+// #define USE_SIMULATOR
 #define USE_PITOT_VIRTUAL
 #define USE_FAKE_BATT_SENSOR