Plane: ensure we init z controller when inactive

this prevents us getting the AC_PosControl internal error, which is
turning up as a common false positive.
This commit is contained in:
Andrew Tridgell 2022-12-03 12:13:13 +11:00
parent 5a5297a893
commit 90001e54d8
1 changed files with 1 additions and 1 deletions

View File

@ -963,7 +963,7 @@ void QuadPlane::run_z_controller(void)
// never run Z controller in tailsitter transtion // never run Z controller in tailsitter transtion
return; return;
} }
if ((now - last_pidz_active_ms) > 20) { if ((now - last_pidz_active_ms) > 20 || !pos_control->is_active_z()) {
// set vertical speed and acceleration limits // set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);