Plane: cope with high angle error in airbrake state

if we are flying too far off the target vector then exit airbrake
state. This prevents flying for a long distance away from the landing
point in airbrake mode
This commit is contained in:
Andrew Tridgell 2022-03-03 14:25:32 +11:00
parent f9ecf15973
commit 3659669409
1 changed files with 1 additions and 0 deletions

View File

@ -2337,6 +2337,7 @@ void QuadPlane::vtol_position_controller(void)
if (poscontrol.get_state() == QPOS_AIRBRAKE && if (poscontrol.get_state() == QPOS_AIRBRAKE &&
poscontrol.time_since_state_start_ms() > min_airbrake_ms && poscontrol.time_since_state_start_ms() > min_airbrake_ms &&
(aspeed < aspeed_threshold || (aspeed < aspeed_threshold ||
fabsf(degrees(closing_vel.angle(desired_closing_vel))) > 60 ||
closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) || closing_speed > MAX(desired_closing_speed*1.2, desired_closing_speed+2) ||
labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd || labs(plane.ahrs.roll_sensor - plane.nav_roll_cd) > attitude_error_threshold_cd ||
labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) { labs(plane.ahrs.pitch_sensor - plane.nav_pitch_cd) > attitude_error_threshold_cd)) {