From 7e49147bcf1473311339a45a92b5026e9640c846 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Tue, 18 Oct 2022 16:05:05 +0200 Subject: [PATCH] Fix feedforward acceleration setpoints for fixedwing offboard position This commit fixes feedforward acceleration setpoints for fixedwing offboard position control. Previously when acceleration feedforward inputs were sent, negative curvature accelerations were not being computed properly --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 503c6b6043..84e1b3017b 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -2161,10 +2161,13 @@ FixedwingPositionControl::Run() 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.velocity[0], trajectory_setpoint.velocity[1]); + Vector2f normalized_velocity_sp_2d = velocity_sp_2d.normalized(); 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(); + Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(normalized_velocity_sp_2d) * + normalized_velocity_sp_2d; + float direction = -normalized_velocity_sp_2d.cross(acceleration_normal.normalized()); + _pos_sp_triplet.current.loiter_radius = direction * velocity_sp_2d.norm() * velocity_sp_2d.norm() / + acceleration_normal.norm(); } else { _pos_sp_triplet.current.loiter_radius = NAN;