ArduCopter: add logging for land detector

ArduCopter/Copter.h

@@ -831,6 +831,28 @@ private:
     void set_land_complete_maybe(bool b);
     void update_throttle_mix();
     bool get_force_flying() const;
+#if HAL_LOGGING_ENABLED
+    enum class LandDetectorLoggingFlag : uint16_t {
+        LANDED               = 1U <<  0,
+        LANDED_MAYBE         = 1U <<  1,
+        LANDING              = 1U <<  2,
+        STANDBY_ACTIVE       = 1U <<  3,
+        WOW                  = 1U <<  4,
+        RANGEFINDER_BELOW_2M = 1U <<  5,
+        DESCENT_RATE_LOW     = 1U <<  6,
+        ACCEL_STATIONARY     = 1U <<  7,
+        LARGE_ANGLE_ERROR    = 1U <<  8,
+        LARGE_ANGLE_REQUEST  = 1U <<  8,
+        MOTOR_AT_LOWER_LIMIT = 1U <<  9,
+        THROTTLE_MIX_AT_MIN  = 1U << 10,
+    };
+    struct {
+        uint32_t last_logged_ms;
+        uint32_t last_logged_count;
+        uint16_t last_logged_flags;
+    } land_detector;
+    void Log_LDET(uint16_t logging_flags, uint32_t land_detector_count);
+#endif
 
 #if AP_LANDINGGEAR_ENABLED
     // landing_gear.cpp

ArduCopter/land_detector.cpp

@@ -44,6 +44,13 @@ void Copter::update_land_detector()
     // range finder :                       tend to be problematic at very short distances
     // input throttle :                     in slow land the input throttle may be only slightly less than hover
 
+#if HAL_LOGGING_ENABLED
+    uint16_t logging_flags = 0;
+#define SET_LOG_FLAG(condition, flag) if (condition) { logging_flags |= (uint16_t)flag; }
+#else
+#define SET_LOG_FLAG(condition, flag)
+#endif
+
     if (!motors->armed()) {
         // if disarmed, always landed.
         set_land_complete(true);
@@ -74,6 +81,7 @@ void Copter::update_land_detector()
 
         // check if landing
         const bool landing = flightmode->is_landing();
+        SET_LOG_FLAG(landing, LandDetectorLoggingFlag::LANDING);
         bool motor_at_lower_limit = (flightmode->has_manual_throttle() && (motors->get_below_land_min_coll() || heli_flags.coll_stk_low) && fabsf(ahrs.get_roll()) < M_PI/2.0f)
 #if MODE_AUTOROTATE_ENABLED == ENABLED
                                     || (flightmode->mode_number() == Mode::Number::AUTOROTATE && motors->get_below_land_min_coll())
@@ -91,6 +99,8 @@ void Copter::update_land_detector()
             throttle_mix_at_min = true;
         }
 #endif
+        SET_LOG_FLAG(motor_at_lower_limit, LandDetectorLoggingFlag::MOTOR_AT_LOWER_LIMIT);
+        SET_LOG_FLAG(throttle_mix_at_min, LandDetectorLoggingFlag::THROTTLE_MIX_AT_MIN);
 
         uint8_t land_detector_scalar = 1;
 #if AP_LANDINGGEAR_ENABLED
@@ -103,19 +113,24 @@ void Copter::update_land_detector()
         // check for aggressive flight requests - requested roll or pitch angle below 15 degrees
         const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
         bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD;
+        SET_LOG_FLAG(large_angle_request, LandDetectorLoggingFlag::LARGE_ANGLE_REQUEST);
 
         // check for large external disturbance - angle error over 30 degrees
         const float angle_error = attitude_control->get_att_error_angle_deg();
         bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
+        SET_LOG_FLAG(large_angle_error, LandDetectorLoggingFlag::LARGE_ANGLE_ERROR);
 
         // check that the airframe is not accelerating (not falling or braking after fast forward flight)
         bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar);
+        SET_LOG_FLAG(accel_stationary, LandDetectorLoggingFlag::ACCEL_STATIONARY);
 
         // check that vertical speed is within 1m/s of zero
         bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100.0 * LAND_DETECTOR_VEL_Z_MAX * land_detector_scalar;
+        SET_LOG_FLAG(descent_rate_low, LandDetectorLoggingFlag::DESCENT_RATE_LOW);
 
         // if we have a healthy rangefinder only allow landing detection below 2 meters
         bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM);
+        SET_LOG_FLAG(rangefinder_check, LandDetectorLoggingFlag::RANGEFINDER_BELOW_2M);
 
         // if we have weight on wheels (WoW) or ambiguous unknown. never no WoW
 #if AP_LANDINGGEAR_ENABLED
@@ -123,6 +138,7 @@ void Copter::update_land_detector()
 #else
         const bool WoW_check = true;
 #endif
+        SET_LOG_FLAG(WoW_check, LandDetectorLoggingFlag::WOW);
 
         if (motor_at_lower_limit && throttle_mix_at_min && !large_angle_request && !large_angle_error && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) {
             // landed criteria met - increment the counter and check if we've triggered
@@ -138,8 +154,53 @@ void Copter::update_land_detector()
     }
 
     set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz()));
+
+#if HAL_LOGGING_ENABLED
+// @LoggerMessage: LDET
+// @Description: Land Detector State
+// @Field: TimeUS: Time since system startup
+// @Field: Flags: boolean state flags
+// @FieldBitmaskEnum: Flags: Copter::LandDetectorLoggingFlag
+// @Field: Count: landing_detector pass count
+    SET_LOG_FLAG(ap.land_complete, LandDetectorLoggingFlag::LANDED);
+    SET_LOG_FLAG(ap.land_complete_maybe, LandDetectorLoggingFlag::LANDED_MAYBE);
+    SET_LOG_FLAG(standby_active, LandDetectorLoggingFlag::STANDBY_ACTIVE);
+    Log_LDET(logging_flags, land_detector_count);
+#undef SET_LOG_FLAG
+#endif
 }
 
+#if HAL_LOGGING_ENABLED
+void Copter::Log_LDET(uint16_t logging_flags, uint32_t detector_count)
+{
+    // do not log if no change:
+    if (logging_flags == land_detector.last_logged_flags &&
+        detector_count == land_detector.last_logged_count) {
+        return;
+    }
+    // do not log more than 50Hz:
+    const auto now = AP_HAL::millis();
+    if (now - land_detector.last_logged_ms < 20) {
+        return;
+    }
+
+    land_detector.last_logged_count = detector_count;
+    land_detector.last_logged_flags = logging_flags;
+    land_detector.last_logged_ms = now;
+
+    AP::logger().WriteStreaming(
+        "LDET",
+        "TimeUS," "Flags," "Count",
+        "s"       "-"      "-",
+        "F"       "-"      "-",
+        "Q"       "H"      "I",
+        AP_HAL::micros64(),
+        logging_flags,
+        land_detector_count
+    );
+}
+#endif
+
 // set land_complete flag and disarm motors if disarm-on-land is configured
 void Copter::set_land_complete(bool b)
 {