fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-12 16:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position Control: handle invalid z or vz measurement in case of nav_state DESCEND

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-10-22 17:18:43 +02:00
parent c342f9baf7
commit 5a53190ed5
1 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+8 -2
View File
@@ -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);