CR123 + CR128 + CR129

docs/Settings.md

@@ -3404,7 +3404,7 @@ Defines at what altitude the descent velocity should start to be `nav_land_minal
 
 ### nav_landing_bump_detection
 
-Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors.
+Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS).
 
 | Default | Min | Max |
 | --- | --- | --- |

src/main/fc/settings.yaml

@@ -2418,7 +2418,8 @@ groups:
         min: 1
         max: 15
       - name: nav_landing_bump_detection
-        description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and currently only works for multirotors."
+        # CR129
+        description: "Allows immediate landing detection based on G bump at touchdown when set to ON. Requires a barometer and GPS and currently only works for multirotors (Note: will work during Failsafe without need for a GPS)."
         default_value: OFF
         field: general.flags.landing_bump_detection
         type: bool

src/main/flight/failsafe.c

@@ -84,7 +84,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
     .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT,                   // Time delay before Failsafe activated during WP mission (s)
 #ifdef USE_GPS_FIX_ESTIMATION
     .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
-#endif    
+#endif
 );
 
 typedef enum {
@@ -350,16 +350,16 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
         }
     }
 
-    // Inhibit Failsafe if emergency landing triggered manually
-    if (posControl.flags.manualEmergLandActive) {
+    // Inhibit Failsafe if emergency landing triggered manually or if landing has been detected
+    if (posControl.flags.manualEmergLandActive || STATE(LANDING_DETECTED)) {  // CR123
         return FAILSAFE_PROCEDURE_NONE;
     }
 
     // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
     // GPS must also be working, and home position set
     if (failsafeConfig()->failsafe_min_distance > 0 &&
-            ((sensors(SENSOR_GPS) && STATE(GPS_FIX)) 
-#ifdef USE_GPS_FIX_ESTIMATION    
+            ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
+#ifdef USE_GPS_FIX_ESTIMATION
                 || STATE(GPS_ESTIMATED_FIX)
 #endif
                 ) && STATE(GPS_FIX_HOME)) {
@@ -429,8 +429,8 @@ void failsafeUpdateState(void)
 #ifdef USE_GPS_FIX_ESTIMATION
                     if ( checkGPSFixFailsafe() ) {
                         reprocessState = true;
-                    } else 
-#endif                    
+                    } else
+#endif
                     if (!receivingRxDataAndNotFailsafeMode) {
                         if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
                             // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
@@ -499,7 +499,7 @@ void failsafeUpdateState(void)
                 } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) {  // trigger new failsafe procedure if changed
                     failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
                     reprocessState = true;
-                } 
+                }
 #ifdef USE_GPS_FIX_ESTIMATION
                 else {
                     if ( checkGPSFixFailsafe() ) {

src/main/navigation/navigation.c

@@ -3353,7 +3353,7 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d
  *-----------------------------------------------------------*/
 void updateLandingStatus(timeMs_t currentTimeMs)
 {
-    // if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
+    // if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {      // CR123
         // return;     // no point using this with a fixed wing if not set to disarm
     // }
 
@@ -3386,8 +3386,14 @@ void updateLandingStatus(timeMs_t currentTimeMs)
             ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
             disarm(DISARM_LANDING);
         } else if (!navigationInAutomaticThrottleMode()) {
-            // for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
-            landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
+            // CR123
+            if (STATE(AIRPLANE) && isFlightDetected()) { // cancel landing detection flag if plane redetected in flight
+                resetLandingDetector();
+            } else {
+                // for multirotor - reactivate landing detector without disarm when throttle raised toward hover throttle
+                landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
+            }
+            // CR123
         }
     } else if (isLandingDetected()) {
         ENABLE_STATE(LANDING_DETECTED);

src/main/navigation/navigation_multicopter.c

@@ -810,18 +810,21 @@ bool isMulticopterCrashedInverted(void)
 {
     static timeMs_t startTime = 0;
 
-    if ((ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) && fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < MC_LAND_CHECK_VEL_Z_MOVING) {
+    if (ABS(attitude.values.roll) > 1000 || ABS(attitude.values.pitch) > 700) {
+        static uint32_t initialAltitude;
+
         if (startTime == 0) {
             startTime = millis();
-        } else {
-            uint16_t disarmTimeDelay = 2000 + S2MS(navConfig()->mc.inverted_crash_detection);
+            initialAltitude = navGetCurrentActualPositionAndVelocity()->pos.z;
+            return false;
+        } else if (ABS(initialAltitude - navGetCurrentActualPositionAndVelocity()->pos.z) < 200) {
+            uint16_t disarmTimeDelay = 3000 + S2MS(navConfig()->mc.inverted_crash_detection);
 
             return millis() - startTime > disarmTimeDelay;
         }
-    } else {
-        startTime = 0;
     }
 
+    startTime = 0;
     return false;
 }
 // CR128
@@ -838,9 +841,15 @@ bool isMulticopterLandingDetected(void)
     }
 // CR128
 #if defined(USE_BARO)
-    if (sensors(SENSOR_BARO) && navConfig()->general.flags.landing_bump_detection && isLandingGbumpDetected(currentTimeMs)) {
+    // CR129
+    bool gBumpDetectionUsable = navConfig()->general.flags.landing_bump_detection && sensors(SENSOR_BARO) &&
+                                ((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
     }
+    // CR129
 #endif
     bool throttleIsBelowMidHover = rcCommand[THROTTLE] < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));