From 6dfe95161e9e94fe967fc7c0d1c59018ead267b7 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 22 Oct 2024 16:49:18 +0200 Subject: [PATCH 1/2] Commander: remove mode_req_local_alt requirement from DESCEND Signed-off-by: Silvan Fuhrer --- src/modules/commander/ModeUtil/mode_requirements.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 32eb5b1e8116..6b72a323d04e 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -115,7 +115,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) // NAVIGATION_STATE_DESCEND setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_attitude); - setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_local_alt); setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_prevent_arming); // NAVIGATION_STATE_TERMINATION From e234b583b75b0500e1584ca4c2dbcceef15eb52e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 22 Oct 2024 17:18:43 +0200 Subject: [PATCH 2/2] 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 | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 4a20c0cf6969..4f8a1a0e9325 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -968,6 +968,12 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); } + // Special case: if z or vz estimate is invalid we cannot control height anymore. To prevent a + // "climb-away" we set the thrust to 0 in that case. + if (!_local_pos.z_valid || !_local_pos.v_z_valid) { + _att_sp.thrust_body[0] = 0.f; + } + 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); @@ -1001,6 +1007,13 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) 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 0 in that case. + if (!_local_pos.v_z_valid) { + _att_sp.thrust_body[0] = 0.f; + } + 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);