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 3c9452621b
commit 99066a27f8

View File

@ -963,7 +963,7 @@ void QuadPlane::run_z_controller(void)
// never run Z controller in tailsitter transtion
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
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);