Initial build

docs/Settings.md

@@ -294,6 +294,7 @@
 | nav_emerg_landing_speed | 500 | 100 | 2000 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] |
 | nav_extra_arming_safety | ON |  |  | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used |
 | nav_fw_allow_manual_thr_increase | OFF |  |  | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing |
+| nav_fw_auto_disarm_delay | 2000 | 100 | 10000 | Delay before plane disarms when `nav_disarm_on_landing` is set (ms) |
 | nav_fw_bank_angle | 35 | 5 | 80 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll |
 | nav_fw_climb_angle | 20 | 5 | 80 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
 | nav_fw_control_smoothness | 0 | 0 | 9 | How smoothly the autopilot controls the airplane to correct the navigation error |
@@ -341,7 +342,7 @@
 | nav_manual_speed | 500 | 10 | 2000 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
 | nav_max_altitude | 0 | 0 | 65000 | Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled |
 | nav_max_terrain_follow_alt | 100 |  | 1000 | Max allowed above the ground altitude for terrain following mode |
-| nav_mc_auto_disarm_delay | 2000 | 100 | 10000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) |
+| nav_mc_auto_disarm_delay | 2000 | 100 | 10000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (ms) |
 | nav_mc_bank_angle | 30 | 15 | 45 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude |
 | nav_mc_braking_bank_angle | 40 | 15 | 60 | max angle that MR is allowed to bank in BOOST mode |
 | nav_mc_braking_boost_disengage_speed | 100 | 0 | 1000 | BOOST will be disabled when speed goes below this value |

src/main/fc/fc_core.c

@@ -930,6 +930,11 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         writeMotors();
     }
 
+    // Check if landed, FW and MR
+    if (STATE(ALTITUDE_CONTROL)) {
+        updateLandingStatus();
+    }
+
 #ifdef USE_BLACKBOX
     if (!cliMode && feature(FEATURE_BLACKBOX)) {
         blackboxUpdate(micros());

src/main/fc/fc_core.h

@@ -30,6 +30,7 @@ typedef enum disarmReason_e {
     DISARM_KILLSWITCH   = 5,
     DISARM_FAILSAFE     = 6,
     DISARM_NAVIGATION   = 7,
+    DISARM_LANDING      = 8,
     DISARM_REASON_COUNT
 } disarmReason_t;
 

src/main/fc/runtime_config.h

@@ -45,15 +45,17 @@ typedef enum {
     ARMING_DISABLED_PWM_OUTPUT_ERROR                = (1 << 27),
     ARMING_DISABLED_NO_PREARM                       = (1 << 28),
     ARMING_DISABLED_DSHOT_BEEPER                    = (1 << 29),
+    ARMING_DISABLED_LANDING_DETECTED                = (1 << 30),
 
-    ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | 
+    ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING |
                                                        ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
                                                        ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED |
                                                        ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE |
                                                        ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI |
                                                        ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED |
                                                        ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING |
-                                                       ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER),
+                                                       ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER |
+                                                       ARMING_DISABLED_LANDING_DETECTED),
 } armingFlag_e;
 
 // Arming blockers that can be overriden by emergency arming.
@@ -134,6 +136,7 @@ typedef enum {
     SET_REVERSIBLE_MOTORS_FORWARD       = (1 << 23),
     FW_HEADING_USE_YAW                  = (1 << 24),
     ANTI_WINDUP_DEACTIVATED             = (1 << 25),
+    LANDING_DETECTED                    = (1 << 26),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/fc/settings.yaml

@@ -2490,7 +2490,7 @@ groups:
         min: 1000
         max: 2000
       - name: nav_mc_auto_disarm_delay
-        description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)"
+        description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (ms)"
         default_value: 2000
         field: mc.auto_disarm_delay
         min: 100
@@ -2588,6 +2588,12 @@ groups:
         field: fw.max_throttle
         min: 1000
         max: 2000
+      - name: nav_fw_auto_disarm_delay
+        description: "Delay before plane disarms when `nav_disarm_on_landing` is set (ms)"
+        default_value: 2000
+        field: fw.auto_disarm_delay
+        min: 100
+        max: 10000
       - name: nav_fw_bank_angle
         description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll"
         default_value: 35

src/main/io/osd.c

@@ -747,6 +747,8 @@ static const char * osdArmingDisabledReasonMessage(void)
         case ARMING_DISABLED_DSHOT_BEEPER:
             return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER);
             // Cases without message
+        case ARMING_DISABLED_LANDING_DETECTED:
+            FALLTHROUGH;
         case ARMING_DISABLED_CMS_MENU:
             FALLTHROUGH;
         case ARMING_DISABLED_OSD_MENU:
@@ -3284,7 +3286,7 @@ static void osdUpdateStats(void)
 
 static void osdShowStatsPage1(void)
 {
-    const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" };
+    const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS", "LANDING"};
     uint8_t top = 1;    /* first fully visible line */
     const uint8_t statNameX = 1;
     const uint8_t statValuesX = 20;
@@ -3873,6 +3875,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     if (FLIGHT_MODE(HEADFREE_MODE)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
                     }
+                    if (STATE(LANDING_DETECTED)) {
+                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_LANDED);
+                    }
                 }
                 // Pick one of the available messages. Each message lasts
                 // a second.

src/main/io/osd_dji_hd.c

@@ -507,6 +507,8 @@ static const char * osdArmingDisabledReasonMessage(void)
         case ARMING_DISABLED_DSHOT_BEEPER:
             return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE");
             // Cases without message
+        case ARMING_DISABLED_LANDING_DETECTED:
+            FALLTHROUGH;
         case ARMING_DISABLED_CMS_MENU:
             FALLTHROUGH;
         case ARMING_DISABLED_OSD_MENU:

src/main/navigation/navigation.c

@@ -57,7 +57,6 @@
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"
 
-
 // Multirotors:
 #define MR_RTH_CLIMB_OVERSHOOT_CM   100  // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
 #define MR_RTH_CLIMB_MARGIN_MIN_CM  100  // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
@@ -197,6 +196,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
         .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
         .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
+        .auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT,          // ms - time delay to disarm when auto disarm after landing enabled
     }
 );
 
@@ -1314,7 +1314,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
     if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
         // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
         if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
-            resetLandingDetector();
+            resetLandingDetector();   // force reset landing detector just in case
             updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
             return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
         }
@@ -1371,7 +1371,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
     if (!ARMING_FLAG(ARMED)) {
         return NAV_FSM_EVENT_SUCCESS;
     }
-    else if (isLandingDetected()) {
+    else if (STATE(LANDING_DETECTED)) {
         return NAV_FSM_EVENT_SUCCESS;
     }
     else {
@@ -1410,7 +1410,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigatio
     UNUSED(previousState);
 
     //On ROVER and BOAT disarm immediately
-    if (!STATE(ALTITUDE_CONTROL) || navConfig()->general.flags.disarm_on_landing) {
+    if (!STATE(ALTITUDE_CONTROL)) {
         disarm(DISARM_NAVIGATION);
     }
 
@@ -1688,20 +1688,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState)
 {
-    // TODO:
     UNUSED(previousState);
+
+    if (STATE(LANDING_DETECTED)) {
+        return NAV_FSM_EVENT_SUCCESS;
+    }
+
     return NAV_FSM_EVENT_NONE;
 }
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState)
 {
-    // TODO:
     UNUSED(previousState);
 
-    // Prevent I-terms growing when already landed
+    rcCommand[THROTTLE] = getThrottleIdleValue();
-    pidResetErrorAccumulators();
+    ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
 
-    return NAV_FSM_EVENT_SUCCESS;
+    return NAV_FSM_EVENT_NONE;
 }
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState)
@@ -2538,28 +2541,56 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc
 /*-----------------------------------------------------------
  * NAV land detector
  *-----------------------------------------------------------*/
-void resetLandingDetector(void)
+void updateLandingStatus(void)
 {
-    if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
+    if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
-        resetFixedWingLandingDetector();
+        return;     // no point using this with a fixed wing if not set to disarm
     }
-    else {
+
-        resetMulticopterLandingDetector();
+    static bool landingDetectorIsActive;
+
+    if (!ARMING_FLAG(ARMED)) {
+        resetLandingDetector();
+        landingDetectorIsActive = false;
+        if (!IS_RC_MODE_ACTIVE(BOXARM)) {
+            DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
+        }
+        return;
+    }
+
+    if (!landingDetectorIsActive) {
+        if (isFlightDetected()) {
+            landingDetectorIsActive = true;
+            resetLandingDetector();
+        }
+    } else if (STATE(LANDING_DETECTED)) {
+        pidResetErrorAccumulators();
+        if (navConfig()->general.flags.disarm_on_landing) {
+            ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
+            disarm(DISARM_LANDING);
+        } else if (!navigationIsFlyingAutonomousMode()) {
+            // for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
+            landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (navConfig()->mc.hover_throttle + getThrottleIdleValue()));
+        }
+    } else if (isLandingDetected()) {
+        ENABLE_STATE(LANDING_DETECTED);
     }
 }
 
 bool isLandingDetected(void)
 {
-    bool landingDetected;
+    return STATE(AIRPLANE) ? isFixedWingLandingDetected() : isMulticopterLandingDetected();
+}
 
-    if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
+void resetLandingDetector(void)
-        landingDetected = isFixedWingLandingDetected();
+{
-    }
+    DISABLE_STATE(LANDING_DETECTED);
-    else {
+    posControl.flags.resetLandingDetector = true;
-        landingDetected = isMulticopterLandingDetected();
+}
-    }
 
-    return landingDetected;
+bool isFlightDetected(void)
+{
+    return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying();
 }
 
 /*-----------------------------------------------------------
@@ -2579,11 +2610,11 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
     }
     else {
 
-        /* 
+        /*
          * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
          * In other words, when altitude is reached, allow it only to shrink
          */
-        if (navConfig()->general.max_altitude > 0 && 
+        if (navConfig()->general.max_altitude > 0 &&
             altitudeToUse >= navConfig()->general.max_altitude &&
             desiredClimbRate > 0
         ) {

src/main/navigation/navigation.h

@@ -275,6 +275,7 @@ typedef struct navConfig_s {
         bool     allow_manual_thr_increase;
         bool    useFwNavYawControl;
         uint8_t yawControlDeadband;
+        uint16_t auto_disarm_delay;          // fixed wing disarm delay for landing detector
     } fw;
 } navConfig_t;
 
@@ -539,6 +540,8 @@ const char * fixedWingLaunchStateMessage(void);
 
 float calculateAverageSpeed(void);
 
+void updateLandingStatus(void);
+
 const navigationPIDControllers_t* getNavigationPIDControllers(void);
 
 int32_t navigationGetHeadingError(void);

src/main/navigation/navigation_fixedwing.c

@@ -35,6 +35,8 @@
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"
+#include "sensors/gyro.h"
+#include "sensors/pitotmeter.h"
 
 #include "flight/pid.h"
 #include "flight/imu.h"
@@ -573,21 +575,64 @@ bool isFixedWingAutoThrottleManuallyIncreased()
     return isAutoThrottleManuallyIncreased;
 }
 
+bool isFixedWingFlying(void)
+{
+    float airspeed = 0;
+#ifdef USE_PITOT
+    airspeed = pitot.airSpeed;
+#endif
+    bool throttleCondition = rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle;
+    bool velCondition = posControl.actualState.velXY > 250 || airspeed > 250;
+
+    return isImuHeadingValid() && throttleCondition && velCondition;
+}
+
 /*-----------------------------------------------------------
  * FixedWing land detector
  *-----------------------------------------------------------*/
-static timeUs_t landingTimerUs;
-
-void resetFixedWingLandingDetector(void)
-{
-    landingTimerUs = micros();
-}
-
 bool isFixedWingLandingDetected(void)
 {
-    timeUs_t currentTimeUs = micros();
+    static bool fixAxisCheck = false;
+    const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
 
-    landingTimerUs = currentTimeUs;
+    // Basic condition to start looking for landing
+    bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
+                          || FLIGHT_MODE(FAILSAFE_MODE)
+                          || (!navigationIsControllingThrottle() && throttleIsLow);
+
+    if (!startCondition || posControl.flags.resetLandingDetector) {
+        return fixAxisCheck = posControl.flags.resetLandingDetector = false;
+    }
+
+    static timeMs_t fwLandingTimerStartAt;
+    static int16_t fwLandSetRollDatum;
+    static int16_t fwLandSetPitchDatum;
+
+    timeMs_t currentTimeMs = millis();
+
+    // Check horizontal and vertical volocities are low (cm/s)
+    bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 50.0f && posControl.actualState.velXY < 100.0f;
+    // Check angular rates are low (degs/s)
+    bool gyroCondition = averageAbsGyroRates() < 2.0f;
+
+    if (velCondition && gyroCondition){
+        if (!fixAxisCheck) {        // capture roll and pitch angles to be used as datums to check for absolute change
+            fwLandSetRollDatum = attitude.values.roll;  //0.1 deg increments
+            fwLandSetPitchDatum = attitude.values.pitch;
+            fixAxisCheck = true;
+            fwLandingTimerStartAt = currentTimeMs;
+        } else {
+            bool isRollAxisStatic = ABS(fwLandSetRollDatum - attitude.values.roll) < 5;
+            bool isPitchAxisStatic = ABS(fwLandSetPitchDatum - attitude.values.pitch) < 5;
+            if (isRollAxisStatic && isPitchAxisStatic) {
+                // Probably landed, low horizontal and vertical velocities and no axis rotation in Roll and Pitch
+                timeMs_t safetyTimeDelay = 2000 + navConfig()->fw.auto_disarm_delay;
+                return currentTimeMs - fwLandingTimerStartAt > safetyTimeDelay; // check conditions stable for 2s + optional extra delay
+            } else {
+                fixAxisCheck = false;
+            }
+        }
+    }
     return false;
 }
 

src/main/navigation/navigation_multicopter.c

@@ -36,6 +36,7 @@
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"
+#include "sensors/gyro.h"
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
@@ -443,11 +444,11 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
      * We override computed speed with max speed in following cases:
      * 1 - computed velocity is > maxSpeed
      * 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed
-     */    
+     */
     if (
-        (navGetCurrentStateFlags() & NAV_AUTO_WP && 
+        (navGetCurrentStateFlags() & NAV_AUTO_WP &&
-        !isApproachingLastWaypoint() && 
+        !isApproachingLastWaypoint() &&
-        newVelTotal < maxSpeed && 
+        newVelTotal < maxSpeed &&
         !navConfig()->mc.slowDownForTurning
         ) || newVelTotal > maxSpeed
     ) {
@@ -672,51 +673,74 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
     }
 }
 
+bool isMulticopterFlying(void)
+{
+    bool throttleCondition = rcCommand[THROTTLE] > navConfig()->mc.hover_throttle;
+    bool gyroCondition = averageAbsGyroRates() > 7.0f;
+
+    return throttleCondition && gyroCondition;
+}
+
 /*-----------------------------------------------------------
  * Multicopter land detector
  *-----------------------------------------------------------*/
-static timeUs_t landingTimer;
-static timeUs_t landingDetectorStartedAt;
-static int32_t landingThrSum;
-static int32_t landingThrSamples;
-
-void resetMulticopterLandingDetector(void)
-{
-    // FIXME: This function is called some time before isMulticopterLandingDetected is first called
-    landingTimer = micros();
-    landingDetectorStartedAt = 0; // ugly hack for now
-
-    landingThrSum = 0;
-    landingThrSamples = 0;
-}
-
 bool isMulticopterLandingDetected(void)
 {
-    const timeUs_t currentTimeUs = micros();
+    static timeUs_t landingDetectorStartedAt;
+    const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
 
-    // FIXME: Remove delay between resetMulticopterLandingDetector and first run of this function so this code isn't needed.
+    /* Basic condition to start looking for landing
-    if (landingDetectorStartedAt == 0) {
+    *  Prevent landing detection if WP mission allowed during Failsafe (except landing states) */
-        landingDetectorStartedAt = currentTimeUs;
+    bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
+                          || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
+                          || (!navigationIsFlyingAutonomousMode() && throttleIsLow);
+
+    if (!startCondition || posControl.flags.resetLandingDetector) {
+        landingDetectorStartedAt = 0;
+        return posControl.flags.resetLandingDetector = false;
     }
 
-    // Average climb rate should be low enough
+    // check vertical and horizontal velocities are low (cm/s)
-    bool verticalMovement = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) > 25.0f;
+    bool velCondition = fabsf(navGetCurrentActualPositionAndVelocity()->vel.z) < 25.0f && posControl.actualState.velXY < 100.0f;
+    // check gyro rates are low (degs/s)
+    bool gyroCondition = averageAbsGyroRates() < 2.0f;
 
-    // check if we are moving horizontally
+    bool possibleLandingDetected = false;
-    bool horizontalMovement = posControl.actualState.velXY > 100.0f;
+    const timeUs_t currentTimeUs = micros();
 
-    // We have likely landed if throttle is 40 units below average descend throttle
+    if (navGetCurrentStateFlags() & NAV_CTL_LAND) {
-    // We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed
+        // We have likely landed if throttle is 40 units below average descend throttle
-    // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core)
+        // We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed
-    // Wait for 1 second so throttle has stabilized.
+        // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core)
-    bool isAtMinimalThrust = false;
+
-    if (currentTimeUs - landingDetectorStartedAt > 1000 * 1000) {
+        static int32_t landingThrSum;
+        static int32_t landingThrSamples;
+        bool isAtMinimalThrust = false;
+
+        if (landingDetectorStartedAt == 0) {
+            landingThrSum = landingThrSamples = 0;
+            landingDetectorStartedAt = currentTimeUs;
+        }
+        if (!landingThrSamples) {
+            if (currentTimeUs - landingDetectorStartedAt < 1000000) {   // Wait for 1 second so throttle has stabilized.
+                return false;
+            } else {
+                landingDetectorStartedAt = currentTimeUs;
+            }
+        }
         landingThrSamples += 1;
         landingThrSum += rcCommandAdjustedThrottle;
         isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - 40);
-    }
 
-    bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
+        possibleLandingDetected = isAtMinimalThrust && velCondition;
+    } else {    // non autonomous and emergency landing
+        if (landingDetectorStartedAt != 0) {
+            possibleLandingDetected = velCondition && gyroCondition;
+        } else {
+            landingDetectorStartedAt = currentTimeUs;
+            return false;
+        }
+    }
 
     // If we have surface sensor (for example sonar) - use it to detect touchdown
     if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) {
@@ -726,13 +750,13 @@ bool isMulticopterLandingDetected(void)
         possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + 5.0f));
     }
 
-    if (!possibleLandingDetected) {
+    if (possibleLandingDetected) {
-        landingTimer = currentTimeUs;
+        timeUs_t safetyTimeDelay = 1000 * (2000 + navConfig()->mc.auto_disarm_delay);  // check conditions stable for 2s + optional extra delay
+        return (currentTimeUs - landingDetectorStartedAt > safetyTimeDelay);
+    } else {
+        landingDetectorStartedAt = currentTimeUs;
         return false;
     }
-    else {
-        return ((currentTimeUs - landingTimer) > (navConfig()->mc.auto_disarm_delay * 1000)) ? true : false;
-    }
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation_private.h

@@ -92,6 +92,9 @@ typedef struct navigationFlags_s {
     bool isTerrainFollowEnabled;            // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings)
 
     bool forcedRTHActivated;
+
+    /* Landing detector */
+    bool resetLandingDetector;
 } navigationFlags_t;
 
 typedef struct {
@@ -389,8 +392,12 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
 bool isThrustFacingDownwards(void);
 uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);
 int32_t calculateBearingToDestination(const fpVector3_t * destinationPos);
-void resetLandingDetector(void);
+
 bool isLandingDetected(void);
+void resetLandingDetector(void);
+bool isFlightDetected(void);
+bool isFixedWingFlying(void);
+bool isMulticopterFlying(void);
 
 navigationFSMStateFlags_t navGetCurrentStateFlags(void);
 
@@ -428,11 +435,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
 
 void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 
-void resetFixedWingLandingDetector(void);
-void resetMulticopterLandingDetector(void);
-
 bool isMulticopterLandingDetected(void);
-bool isFixedWingLandingDetected(void);
 
 void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
 
@@ -451,6 +454,8 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs);
 float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingError, bool errorIsDecreasing);
 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
 
+bool isFixedWingLandingDetected(void);
+
 void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
 
 /* Fixed-wing launch controller */

src/main/sensors/gyro.h

@@ -101,3 +101,4 @@ bool gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
 void gyroUpdateDynamicLpf(float cutoffFreq);
+float averageAbsGyroRates(void);