From 797dfdd0ed377ad68637a9063e682585cdf8a62c Mon Sep 17 00:00:00 2001 From: lthall Date: Tue, 23 Apr 2024 03:37:26 +0930 Subject: [PATCH] Copter: Land Detector: Include angle checks --- ArduCopter/land_detector.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index d7b82c497d..95743cfafa 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -100,6 +100,14 @@ void Copter::update_land_detector() } #endif + // 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; + + // 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); + // 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); @@ -116,7 +124,7 @@ void Copter::update_land_detector() const bool WoW_check = true; #endif - if (motor_at_lower_limit && throttle_mix_at_min && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) { + 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 if( land_detector_count < land_trigger_sec*scheduler.get_loop_rate_hz()) { land_detector_count++;