Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Land Detector: Include angle checks

Browse Source
This commit is contained in:
lthall 2024-04-23 03:37:26 +09:30 committed by Randy Mackay
parent 25b10fb8c4
commit 797dfdd0ed
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/land_detector.cpp
View File

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