From 5a53190ed51dec216a21e79fca0fd2cfa44f1178 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 22 Oct 2024 17:18:43 +0200 Subject: [PATCH] FW Position Control: handle invalid z or vz measurement in case of nav_state DESCEND Signed-off-by: Silvan Fuhrer --- .../fw_pos_control/FixedwingPositionControl.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 4a20c0cf69..52acb019aa 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -961,7 +961,9 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle const float yaw_body = 0.f; - if (_landed) { + // Special case: if z or vz estimate is invalid we cannot control height anymore. To prevent a + // "climb-away" we set the thrust to MIN in that case. + if (_landed || !_local_pos.z_valid || !_local_pos.v_z_valid) { _att_sp.thrust_body[0] = _param_fw_thr_min.get(); } else { @@ -1000,7 +1002,11 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle const float yaw_body = 0.f; - _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); + // Special case: if vz estimate is invalid we cannot control height rate anymore. To prevent a + // "climb-away" we set the thrust to MIN in that case. + _att_sp.thrust_body[0] = (_landed + || !_local_pos.v_z_valid) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); + const float pitch_body = get_tecs_pitch(); const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); attitude_setpoint.copyTo(_att_sp.q_d);