From 624d8a58e96eb5e85f7dc13d6e8fb8eed9f56897 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Jul 2020 21:10:58 +0200 Subject: [PATCH] FlightTaskOrbit: correct acceleration feed-forward The acceleration setpoint gets implicitly inherited from the altitude flight task since #14212. This feed-forward adds an unwanted acceleration when the right stick is deflected. Instead I'm using it to command the expected centripetal acceleration when flying in a circle for better orbit tracking. --- src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index f9abb1defa..55e58e03c6 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -223,9 +223,9 @@ void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position) // xy velocity adjustment to stay on the radius distance velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero(); - _velocity_setpoint(0) = velocity_xy(0); - _velocity_setpoint(1) = velocity_xy(1); _position_setpoint(0) = _position_setpoint(1) = NAN; + _velocity_setpoint.xy() = velocity_xy; + _acceleration_setpoint.xy() = -center_to_position.unit_or_zero() * _v * _v / _r; } void FlightTaskOrbit::generate_circle_yaw_setpoints(Vector2f center_to_position)