From 85ccc064bdb95e19b55a36facb08a4c61edf69fa Mon Sep 17 00:00:00 2001 From: Silvan Date: Tue, 22 Jul 2025 13:12:42 +0200 Subject: [PATCH] FW Mode Manager: handle loiter radius and direction correctly if not from position_sp In that case we take radius and direction from the param NAV_LOITER_RAD, CCw if negative. Signed-off-by: Silvan --- .../fw_mode_manager/FixedWingModeManager.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index 648f809eff..5ee3c817fd 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -868,10 +868,12 @@ FixedWingModeManager::control_auto_loiter(const float control_interval, const Ve // current waypoint (the one currently heading for) const Vector2d curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - float loiter_radius = pos_sp_curr.loiter_radius; + float loiter_radius = fabsf(pos_sp_curr.loiter_radius); + bool loiter_direction_ccw = pos_sp_curr.loiter_direction_counter_clockwise; - if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) { - loiter_radius = _param_nav_loiter_rad.get(); + if (loiter_radius < FLT_EPSILON) { + loiter_radius = fabsf(_param_nav_loiter_rad.get()); + loiter_direction_ccw = _param_nav_loiter_rad.get() < -FLT_EPSILON; } Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; @@ -902,7 +904,7 @@ FixedWingModeManager::control_auto_loiter(const float control_interval, const Ve } const DirectionalGuidanceOutput sp = navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, - pos_sp_curr.loiter_direction_counter_clockwise, + loiter_direction_ccw, ground_speed, _wind_vel); @@ -1619,10 +1621,12 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons // flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get()); - float loiter_radius = pos_sp_curr.loiter_radius; + float loiter_radius = fabsf(pos_sp_curr.loiter_radius); + bool loiter_direction_ccw = pos_sp_curr.loiter_direction_counter_clockwise; - if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) { - loiter_radius = _param_nav_loiter_rad.get(); + if (loiter_radius < FLT_EPSILON) { + loiter_radius = fabsf(_param_nav_loiter_rad.get()); + loiter_direction_ccw = _param_nav_loiter_rad.get() < -FLT_EPSILON; } if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { @@ -1644,7 +1648,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons /* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */ const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, - pos_sp_curr.loiter_direction_counter_clockwise, + loiter_direction_ccw, ground_speed, _wind_vel); fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; fw_lateral_ctrl_sp.timestamp = now; @@ -1702,7 +1706,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons // follow the glide slope const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius, - pos_sp_curr.loiter_direction_counter_clockwise, + loiter_direction_ccw, ground_speed, _wind_vel); fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; fw_lateral_ctrl_sp.timestamp = now;