Merge pull request #256 from iNavFlight/theArchLadder-landing-detector-avg-thr

Landing detector: replace mc_min_fly_throttle with average throttle

src/main/config/config.c

@@ -278,7 +278,7 @@ void resetNavConfig(navConfig_t * navConfig)
     // MC-specific
     navConfig->mc_max_bank_angle = 30;      // 30 deg
     navConfig->mc_hover_throttle = 1500;
-    navConfig->mc_min_fly_throttle = 1200;
+    navConfig->mc_auto_disarm_delay = 2000;
 
     // Fixed wing
     navConfig->fw_max_bank_angle = 20;      // 30 deg

src/main/flight/navigation_rewrite.c

@@ -1640,13 +1640,14 @@ void calculateFarAwayTarget(t_fp_vector * farAwayPos, int32_t yaw, int32_t dista
 /*-----------------------------------------------------------
  * NAV land detector
  *-----------------------------------------------------------*/
-static uint32_t landingTimer;
-static bool hasHadSomeVelocity;
-
 void resetLandingDetector(void)
 {
-    landingTimer = micros();
+    if (STATE(FIXED_WING)) { // FIXED_WING
-    hasHadSomeVelocity = false;
+        resetFixedWingLandingDetector();
+    }
+    else {
+        resetMulticopterLandingDetector();
+    }
 }
 
 bool isLandingDetected(void)
@@ -1654,10 +1655,10 @@ bool isLandingDetected(void)
     bool landingDetected;
 
     if (STATE(FIXED_WING)) { // FIXED_WING
-        landingDetected = isFixedWingLandingDetected(&landingTimer);
+        landingDetected = isFixedWingLandingDetected();
     }
     else {
-        landingDetected = isMulticopterLandingDetected(&landingTimer, &hasHadSomeVelocity);
+        landingDetected = isMulticopterLandingDetected();
     }
 
     return landingDetected;

src/main/flight/navigation_rewrite.h

@@ -115,7 +115,7 @@ typedef struct navConfig_s {
 
     uint8_t  mc_max_bank_angle;             // multicopter max banking angle (deg)
     uint16_t mc_hover_throttle;             // multicopter hover throttle
-    uint16_t mc_min_fly_throttle;           // multicopter minimum throttle to consider machine flying
+    uint16_t mc_auto_disarm_delay;          // multicopter safety delay for landing detector
 
     uint16_t fw_cruise_throttle;            // Cruise throttle
     uint16_t fw_min_throttle;               // Minimum allowed throttle in auto mode

src/main/flight/navigation_rewrite_fixedwing.c

@@ -446,13 +446,18 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
 /*-----------------------------------------------------------
  * FixedWing land detector
  *-----------------------------------------------------------*/
-bool isFixedWingLandingDetected(uint32_t * landingTimer)
+static uint32_t landingTimer;
+
+void resetFixedWingLandingDetector(void)
+{
+    landingTimer = micros();
+}
+
+bool isFixedWingLandingDetected(void)
 {
     uint32_t currentTime = micros();
 
-    // TODO
+    landingTimer = currentTime;
-
-    *landingTimer = currentTime;
     return false;
 }
 

src/main/flight/navigation_rewrite_multicopter.c

@@ -469,12 +469,29 @@ static void applyMulticopterPositionController(uint32_t currentTime)
 /*-----------------------------------------------------------
  * Multicopter land detector
  *-----------------------------------------------------------*/
-bool isMulticopterLandingDetected(uint32_t * landingTimer, bool * hasHadSomeVelocity)
+static uint32_t landingTimer;
+static uint32_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)
 {
     uint32_t currentTime = micros();
-    
+
-    // When descend stage is activated velocity is ~0, so wait until we have descended faster than -25cm/s
+    // FIXME: Remove delay between resetMulticopterLandingDetector and first run of this function so this code isn't needed.
-    if (!*hasHadSomeVelocity && posControl.actualState.vel.V.Z < -25.0f) *hasHadSomeVelocity = true;
+    if (landingDetectorStartedAt == 0) {
+        landingDetectorStartedAt = currentTime;
+    }
 
     // Average climb rate should be low enough
     bool verticalMovement = fabsf(posControl.actualState.vel.V.Z) > 25.0f;
@@ -482,27 +499,37 @@ bool isMulticopterLandingDetected(uint32_t * landingTimer, bool * hasHadSomeVelo
     // check if we are moving horizontally
     bool horizontalMovement = posControl.actualState.velXY > 100.0f;
 
-    // Throttle should be low enough
+    // We have likely landed if throttle is 40 units below average descend throttle
     // We use rcCommandAdjustedThrottle to keep track of NAV corrected throttle (isLandingDetected is executed
     // from processRx() and rcCommand at that moment holds rc input, not adjusted values from NAV core)
-    bool minimalThrust = rcCommandAdjustedThrottle < posControl.navConfig->mc_min_fly_throttle;
+    // Wait for 1 second so throttle has stabilized.
-    
+    bool isAtMinimalThrust = false;
-    bool possibleLandingDetected = hasHadSomeVelocity && minimalThrust && !verticalMovement && !horizontalMovement;
+    if (currentTime - landingDetectorStartedAt > 1000 * 1000) {
+        landingThrSamples += 1;
+        landingThrSum += rcCommandAdjustedThrottle;
+        isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - 40);
+    }
+
+    bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
     
+    navDebug[0] = isAtMinimalThrust * 100 + !verticalMovement * 10 + !horizontalMovement * 1;
+    navDebug[1] = (landingThrSamples == 0) ? (navDebug[1] = 0) : (rcCommandAdjustedThrottle - (landingThrSum / landingThrSamples));
+    navDebug[2] = (currentTime - landingTimer) / 1000;
+
     // If we have surface sensor (for example sonar) - use it to detect touchdown
     if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface >= 0 && posControl.actualState.surfaceMin >= 0) {
         // TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.
         // TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy.
         // surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed
-        possibleLandingDetected = possibleLandingDetected && posControl.actualState.surface <= (posControl.actualState.surfaceMin + 5.0f);
+        possibleLandingDetected = possibleLandingDetected && (posControl.actualState.surface <= (posControl.actualState.surfaceMin + 5.0f));
     }
 
     if (!possibleLandingDetected) {
-        *landingTimer = currentTime;
+        landingTimer = currentTime;
         return false;
     }
     else {
-        return ((currentTime - *landingTimer) > (LAND_DETECTOR_TRIGGER_TIME_MS * 1000)) ? true : false;
+        return ((currentTime - landingTimer) > (posControl.navConfig->mc_auto_disarm_delay * 1000)) ? true : false;
     }
 }
 

src/main/flight/navigation_rewrite_private.h

@@ -23,7 +23,6 @@
 
 #include "config/runtime_config.h"
 
-#define LAND_DETECTOR_TRIGGER_TIME_MS       2000        // 2 seconds
 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
 #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
 #define NAV_ACCEL_CUTOFF_FREQUENCY_HZ       2       // low-pass filter on XY-acceleration target
@@ -329,7 +328,12 @@ bool adjustMulticopterPositionFromRCInput(void);
 
 void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime);
 
-bool isMulticopterLandingDetected(uint32_t * landingTimer, bool * hasHadSomeVelocity);
+void resetFixedWingLandingDetector(void);
+void resetMulticopterLandingDetector(void);
+
+bool isMulticopterLandingDetected(void);
+bool isFixedWingLandingDetected(void);
+
 void calculateMulticopterInitialHoldPosition(t_fp_vector * pos);
 
 /* Fixed-wing specific functions */
@@ -345,7 +349,6 @@ bool adjustFixedWingPositionFromRCInput(void);
 
 void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, uint32_t currentTime);
 
-bool isFixedWingLandingDetected(uint32_t * landingTimer);
 void calculateFixedWingInitialHoldPosition(t_fp_vector * pos);
 
 #endif

src/main/io/serial_cli.c

@@ -635,7 +635,7 @@ const clivalue_t valueTable[] = {
 
     { "nav_mc_bank_angle",          VAR_UINT8  | MASTER_VALUE, &masterConfig.navConfig.mc_max_bank_angle, .config.minmax = { 15,  45 }, 0 },
     { "nav_mc_hover_thr",           VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.mc_hover_throttle, .config.minmax = { 1000,  2000 }, 0 },
-    { "nav_mc_min_fly_thr",         VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.mc_min_fly_throttle, .config.minmax = { 1000,  2000 }, 0 },
+    { "nav_mc_auto_disarm_delay",   VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.mc_auto_disarm_delay, .config.minmax = { 100,  10000 }, 0 },
 
     { "nav_fw_cruise_thr",          VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_cruise_throttle, .config.minmax = { 1000,  2000 }, 0 },
     { "nav_fw_min_thr",             VAR_UINT16 | MASTER_VALUE, &masterConfig.navConfig.fw_min_throttle, .config.minmax = { 1000,  2000 }, 0 },