From 92598177d0991fc2f04e66d05809c18db31b7a80 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 6 Jul 2020 16:34:39 +0200 Subject: [PATCH 1/3] FlightTaskOrbit: reset reached when new parameters are set such that they get applied to the straight line trajectory class and the drone starts approaching the new orbit. --- src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 55e58e03c6a2..15b396535bea 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -94,6 +94,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) // perpendicularly approach the orbit circle again when new parameters get commanded _in_circle_approach = true; + _circle_approach_line.reset(); return ret; } From 04d46a39c4d666d65954a4d6206dd68bbde5c01d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 6 Jul 2020 16:33:29 +0200 Subject: [PATCH 2/3] StraightLine: allow resetting reached flag --- src/lib/flight_tasks/tasks/Utility/StraightLine.cpp | 4 ++-- src/lib/flight_tasks/tasks/Utility/StraightLine.hpp | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/lib/flight_tasks/tasks/Utility/StraightLine.cpp b/src/lib/flight_tasks/tasks/Utility/StraightLine.cpp index db38e522cfe8..eb4972687856 100644 --- a/src/lib/flight_tasks/tasks/Utility/StraightLine.cpp +++ b/src/lib/flight_tasks/tasks/Utility/StraightLine.cpp @@ -76,7 +76,7 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix // check if we plan to go against the line direction which indicates we reached the goal if (start_to_end * vehicle_to_end < 0) { - end_reached = true; + _end_reached = true; } } @@ -85,6 +85,6 @@ void StraightLine::setLineFromTo(const Vector3f &start, const Vector3f &end) if (PX4_ISFINITE(start.norm_squared()) && PX4_ISFINITE(end.norm_squared())) { _start = start; _end = end; - end_reached = false; + _end_reached = false; } } diff --git a/src/lib/flight_tasks/tasks/Utility/StraightLine.hpp b/src/lib/flight_tasks/tasks/Utility/StraightLine.hpp index 5097999cde20..cdd776da06a1 100644 --- a/src/lib/flight_tasks/tasks/Utility/StraightLine.hpp +++ b/src/lib/flight_tasks/tasks/Utility/StraightLine.hpp @@ -66,7 +66,9 @@ class StraightLine * * @return false when on the way from start to end, true when end was reached */ - bool isEndReached() { return end_reached; } + bool isEndReached() const { return _end_reached; } + + void reset() { _end_reached = true; } private: const matrix::Vector3f &_position; /**< vehicle position (dependency injection) */ @@ -75,5 +77,5 @@ class StraightLine matrix::Vector3f _end; /**< End point of the straight line */ float _speed = 1.f; /**< desired speed between accelerating and decelerating */ - bool end_reached = true; /**< Flag to lock further movement when end is reached */ + bool _end_reached = true; /**< Flag to lock further movement when end is reached */ }; From f6a41d958b483a00162d0ef736a2c52f83bd76ca Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 8 Jul 2020 18:49:46 +0200 Subject: [PATCH 3/3] FlightTaskOrbit: reset approach if radius gets changed --- src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp index 15b396535bea..641a49ae740c 100644 --- a/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -127,6 +127,10 @@ bool FlightTaskOrbit::setRadius(float r) _v = sign(_v) * sqrtf(_acceleration_max * r); } + if (fabs(_r - r) > FLT_EPSILON) { + _circle_approach_line.reset(); + } + _r = r; return true; }