From 8f7cde12ab1a9589afff7825508ed22edb0a4146 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 30 Jul 2024 18:13:14 +1000 Subject: [PATCH] ArduCopter: add logging for land detector --- ArduCopter/Copter.h | 24 +++++++++++++- ArduCopter/land_detector.cpp | 61 ++++++++++++++++++++++++++++++++++++ 2 files changed, 84 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 3ef4c4bae7..fcda06e571 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -831,7 +831,29 @@ 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 void landinggear_update(); diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 37b2cbcf37..ee56446a3a 100644 --- a/ArduCopter/land_detector.cpp +++ b/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) {