Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: add logging for land detector

This commit is contained in:
Peter Barker 2024-07-30 18:13:14 +10:00 committed by Andrew Tridgell
parent 6279f67150
commit 8f7cde12ab
2 changed files with 84 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/Copter.h
View File

@ -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

61
ArduCopter/land_detector.cpp
View File

@ -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)
{