From 925c04d51a07479a9ac1fe6ac6ed17b7ae0e9949 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Thu, 19 Feb 2026 12:18:07 -0800 Subject: [PATCH] Fix regression with gliding setpoints --- .../fw_mode_manager/FixedWingModeManager.cpp | 25 +++++++++++++++---- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 9cbcd2ea4c..e26a5fb585 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -386,8 +386,7 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now) if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid && _control_mode.flag_control_position_enabled) { - if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy) - && PX4_ISFINITE(_pos_sp_triplet.current.vz)) { + if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)) { // Offboard position with velocity setpoints _control_mode_current = FW_POSCTRL_MODE_AUTO_PATH; return; @@ -1082,6 +1081,8 @@ FixedWingModeManager::control_auto_path(const float control_interval, const Vect _ctrl_configuration_handler.setThrottleMin(0.0f); _ctrl_configuration_handler.setThrottleMax(0.0f); _ctrl_configuration_handler.setSpeedWeight(2.0f); + } else { + _ctrl_configuration_handler.setSpeedWeight(1.0f); } } @@ -2089,8 +2090,9 @@ FixedWingModeManager::Run() _pos_sp_triplet.current.lat = static_cast(NAN); _pos_sp_triplet.current.lon = static_cast(NAN); _pos_sp_triplet.current.alt = NAN; + _pos_sp_triplet.current.gliding_enabled = false; - if (Vector3f(trajectory_setpoint.position).isAllFinite()) { + if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1])) { if (_global_local_proj_ref.isInitialized()) { double lat; double lon; @@ -2104,12 +2106,11 @@ FixedWingModeManager::Run() } - if (Vector3f(trajectory_setpoint.velocity).isAllFinite()) { + if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1])) { valid_setpoint = true; _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0]; _pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1]; - _pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2]; if (Vector3f(trajectory_setpoint.acceleration).isAllFinite()) { Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); @@ -2126,6 +2127,20 @@ FixedWingModeManager::Run() } } + if (PX4_ISFINITE(trajectory_setpoint.position[2])) { + if (_global_local_proj_ref.isInitialized()) { + _pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2]; + } + } + + if (PX4_ISFINITE(trajectory_setpoint.velocity[2])) { + _pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2]; + } + + if (!PX4_ISFINITE(trajectory_setpoint.position[2]) && !PX4_ISFINITE(trajectory_setpoint.velocity[2])) { + _pos_sp_triplet.current.gliding_enabled = true; + } + _position_setpoint_current_valid = valid_setpoint; }