Add landing debug

src/main/build/debug.h

@@ -86,5 +86,6 @@ typedef enum {
     DEBUG_AUTOTRIM,
     DEBUG_AUTOTUNE,
     DEBUG_RATE_DYNAMICS,
+    DEBUG_LANDING,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/settings.yaml

@@ -96,7 +96,7 @@ tables:
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
       "IRLOCK", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "IMU2", "ALTITUDE",
-      "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"]
+      "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator

src/main/navigation/navigation.c

@@ -2601,6 +2601,9 @@ void updateLandingStatus(void)
 
     static bool landingDetectorIsActive;
 
+    DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive);
+    DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED));
+
     if (!ARMING_FLAG(ARMED)) {
         resetLandingDetector();
         landingDetectorIsActive = false;

src/main/navigation/navigation_fixedwing.c

@@ -612,6 +612,7 @@ bool isFixedWingFlying(void)
  *-----------------------------------------------------------*/
 bool isFixedWingLandingDetected(void)
 {
+    DEBUG_SET(DEBUG_LANDING, 4, 0);
     static bool fixAxisCheck = false;
     const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
 
@@ -623,6 +624,7 @@ bool isFixedWingLandingDetected(void)
     if (!startCondition || posControl.flags.resetLandingDetector) {
         return fixAxisCheck = posControl.flags.resetLandingDetector = false;
     }
+    DEBUG_SET(DEBUG_LANDING, 4, 1);
 
     static timeMs_t fwLandingTimerStartAt;
     static int16_t fwLandSetRollDatum;
@@ -634,8 +636,12 @@ bool isFixedWingLandingDetected(void)
     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;
+    DEBUG_SET(DEBUG_LANDING, 2, velCondition);
+    DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
 
     if (velCondition && gyroCondition){
+        DEBUG_SET(DEBUG_LANDING, 4, 2);
+        DEBUG_SET(DEBUG_LANDING, 5, fixAxisCheck);
         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;
@@ -644,6 +650,8 @@ bool isFixedWingLandingDetected(void)
         } else {
             bool isRollAxisStatic = ABS(fwLandSetRollDatum - attitude.values.roll) < 5;
             bool isPitchAxisStatic = ABS(fwLandSetPitchDatum - attitude.values.pitch) < 5;
+            DEBUG_SET(DEBUG_LANDING, 6, isRollAxisStatic);
+            DEBUG_SET(DEBUG_LANDING, 7, isPitchAxisStatic);
             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;

src/main/navigation/navigation_multicopter.c

@@ -726,6 +726,7 @@ bool isMulticopterFlying(void)
  *-----------------------------------------------------------*/
 bool isMulticopterLandingDetected(void)
 {
+    DEBUG_SET(DEBUG_LANDING, 4, 0);
     static timeUs_t landingDetectorStartedAt;
     const bool throttleIsLow = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
 
@@ -745,6 +746,8 @@ bool isMulticopterLandingDetected(void)
                         posControl.actualState.velXY < MC_LAND_CHECK_VEL_XY_MOVING;
     // check gyro rates are low (degs/s)
     bool gyroCondition = averageAbsGyroRates() < 2.0f;
+    DEBUG_SET(DEBUG_LANDING, 2, velCondition);
+    DEBUG_SET(DEBUG_LANDING, 3, gyroCondition);
 
     bool possibleLandingDetected = false;
     const timeUs_t currentTimeUs = micros();
@@ -753,6 +756,7 @@ bool isMulticopterLandingDetected(void)
         // 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)
+        DEBUG_SET(DEBUG_LANDING, 4, 1);
 
         static int32_t landingThrSum;
         static int32_t landingThrSamples;
@@ -774,7 +778,11 @@ bool isMulticopterLandingDetected(void)
         isAtMinimalThrust = rcCommandAdjustedThrottle < (landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE);
 
         possibleLandingDetected = isAtMinimalThrust && velCondition;
+
+        DEBUG_SET(DEBUG_LANDING, 6, rcCommandAdjustedThrottle);
+        DEBUG_SET(DEBUG_LANDING, 7, landingThrSum / landingThrSamples - MC_LAND_DESCEND_THROTTLE);
     } else {    // non autonomous and emergency landing
+        DEBUG_SET(DEBUG_LANDING, 4, 2);
         if (landingDetectorStartedAt) {
             possibleLandingDetected = velCondition && gyroCondition;
         } else {
@@ -790,6 +798,7 @@ bool isMulticopterLandingDetected(void)
         // surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed
         possibleLandingDetected = possibleLandingDetected && (posControl.actualState.agl.pos.z <= (posControl.actualState.surfaceMin + MC_LAND_SAFE_SURFACE));
     }
+    DEBUG_SET(DEBUG_LANDING, 5, possibleLandingDetected);
 
     if (possibleLandingDetected) {
         timeUs_t safetyTimeDelay = MS2US(2000 + navConfig()->mc.auto_disarm_delay);  // check conditions stable for 2s + optional extra delay