Skip to content

Commit

Permalink
ArduCopter: add logging for land detector
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker @tridge
peterbarker authored and tridge committed Aug 5, 2024
1 parent 6279f67 commit 8f7cde1
Show file tree
Hide file tree
Showing 2 changed files with 84 additions and 1 deletion.
24 changes: 23 additions & 1 deletion ArduCopter/Copter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -831,7 +831,29 @@ class Copter : public AP_Vehicle {
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();
Expand Down
61 changes: 61 additions & 0 deletions ArduCopter/land_detector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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())
Expand All @@ -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
Expand All @@ -103,26 +113,32 @@ 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
const bool WoW_check = (landinggear.get_wow_state() == AP_LandingGear::LG_WOW || landinggear.get_wow_state() == AP_LandingGear::LG_WOW_UNKNOWN);
#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
Expand All @@ -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)
{
Expand Down

0 comments on commit 8f7cde1

Please sign in to comment.