From 3a9c5c3178dcaf12790c1a6e885e6e8d3b3db4ee Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Mon, 24 Jan 2022 17:16:13 +0100 Subject: [PATCH] Use acceleration to pass path curvature --- .../FixedwingPositionControl.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e69499a2df..35b05b65df 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1195,7 +1195,9 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) { // Navigate directly on position setpoint and path tangent matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy); - _npfg.navigatePathTangent(curr_pos, curr_wp, velocity_2d.normalized(), ground_speed, _wind_vel, pos_sp_curr.yawspeed); + 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); } else { _npfg.navigateWaypoints(prev_wp, curr_wp, curr_pos, ground_speed, _wind_vel); @@ -2263,6 +2265,18 @@ FixedwingPositionControl::Run() _pos_sp_triplet.current.vx = trajectory_setpoint.vx; _pos_sp_triplet.current.vy = trajectory_setpoint.vy; _pos_sp_triplet.current.vz = trajectory_setpoint.vz; + + if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1]) + && PX4_ISFINITE(trajectory_setpoint.acceleration[2])) { + Vector2f velocity_sp_2d(trajectory_setpoint.vx, trajectory_setpoint.vy); + Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]); + Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(velocity_sp_2d) * + velocity_sp_2d.normalized(); + _pos_sp_triplet.current.loiter_radius = velocity_sp_2d.norm() * velocity_sp_2d.norm() / acceleration_normal.norm(); + + } else { + _pos_sp_triplet.current.loiter_radius = NAN; + } } if (!valid_setpoint) {