FW Position Control: handle invalid z or vz measurement in case of na…

…v_state DESCEND

Signed-off-by: Silvan Fuhrer <[email protected]>

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);