From 2fc95bb369483b80804e713421fca4d74524376c Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Wed, 26 Jan 2022 16:41:00 +0100 Subject: [PATCH] Rebase fix Use getnavspeed_2d for groundspeed --- src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 35b05b65df..f29a4361f0 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1197,13 +1197,13 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius : 0.0f; - _npfg.navigatePathTangent(curr_pos, curr_wp, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); + _npfg.navigatePathTangent(curr_pos, curr_wp, velocity_2d.normalized(), get_nav_speed_2d(ground_speed), _wind_vel, + curvature); } else { - _npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel); + _npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed), _wind_vel); } - _npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed), _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas;