Surface sensor (SONAR) assisted landing detection

src/main/flight/navigation_rewrite.c

@@ -1010,6 +1010,15 @@ void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, flo
     posControl.actualState.surface = surfaceDistance;
     posControl.actualState.surfaceVel = surfaceVelocity;
 
+    if (surfaceDistance > 0) {
+        if (posControl.actualState.surfaceMin > 0) {
+            posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, surfaceDistance);
+        }
+        else {
+            posControl.actualState.surfaceMin = surfaceDistance;
+        }
+    }
+
     posControl.flags.hasValidSurfaceSensor = hasValidSensor;
 
     if (hasValidSensor) {
@@ -1785,6 +1794,11 @@ void navigationInit(navConfig_t *initialnavConfig,
     posControl.waypointCount = 0;
     posControl.activeWaypointIndex = 0;
 
+    /* Set initial surface invalid */
+    posControl.actualState.surface = -1.0f;
+    posControl.actualState.surfaceVel = 0.0f;
+    posControl.actualState.surfaceMin = -1.0f;
+
     /* Use system config */
     navigationUseConfig(initialnavConfig);
     navigationUsePIDs(initialPidProfile);

src/main/flight/navigation_rewrite_multicopter.c

@@ -424,7 +424,17 @@ bool isMulticopterLandingDetected(uint32_t * landingTimer)
     // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core)
     bool minimalThrust = rcCommandAdjustedThrottle <= (posControl.escAndServoConfig->minthrottle + (posControl.escAndServoConfig->maxthrottle - posControl.escAndServoConfig->minthrottle) * 0.25f);
 
-    if (!minimalThrust || verticalMovement || horizontalMovement) {
+    bool possibleLandingDetected = false;
+
+    if (posControl.flags.hasValidSurfaceSensor) {
+        /* surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed */
+        possibleLandingDetected = (posControl.actualState.surface <= (posControl.actualState.surfaceMin + 5.0f));
+    }
+    else {
+        possibleLandingDetected = (minimalThrust && !verticalMovement && !horizontalMovement);
+    }
+
+    if (!possibleLandingDetected) {
         *landingTimer = currentTime;
         return false;
     }

src/main/flight/navigation_rewrite_private.h

@@ -21,8 +21,8 @@
 
 #define DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR    1.113195f  // MagicEarthNumber from APM
 
-#define LAND_DETECTOR_TRIGGER_TIME_MS       2000
+#define LAND_DETECTOR_TRIGGER_TIME_MS       2000            // 2 seconds
-#define NAV_ROLL_PITCH_MAX_DECIDEGREES      (30 * 10)      // Max control input from NAV (30 deg)
+#define NAV_ROLL_PITCH_MAX_DECIDEGREES      (30 * 10)       // Max control input from NAV (30 deg)
 
 #define POSITION_TARGET_UPDATE_RATE_HZ      5       // Rate manual position target update (minumum possible speed in cms will be this value)
 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
@@ -108,6 +108,7 @@ typedef struct {
     float       cosYaw;
     float       surface;
     float       surfaceVel;
+    float       surfaceMin;
 } navigationEstimatedState_t;
 
 typedef struct {