From 7774b150a9e9e22c5e7ba08a2e3b6cb5149fd37c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 12 Aug 2019 11:08:11 +0200 Subject: [PATCH] FlightTaskOrbit: only reapproach circle if center changed The condition that the vehicle is more than 3m away from the circle line was too sloppy. That often happens when the radius is changed by sticks. A reapproach is only necessary when the center is moved and that's only possible through the orbit command. --- src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp index 38cdb007a3..4d6c9c6378 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -92,6 +92,9 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) } } + // perpendicularly approach the orbit circle again when new parameters get commanded + _in_circle_approach = true; + return ret; } @@ -179,12 +182,7 @@ bool FlightTaskOrbit::update() setVelocity(v); Vector2f center_to_position = Vector2f(_position) - _center; - const float distance_to_center = center_to_position.norm(); - // perpendicularly approach the orbit circle if further away than 3 meters - if (!_in_circle_approach && !math::isInRange(distance_to_center, _r - 3.f, _r + 3.f)) { - _in_circle_approach = true; - } if (_in_circle_approach) { generate_circle_approach_setpoints();