From cca7596bcd6c1480cb3ecdb7d0483160efb8d714 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 8 Aug 2019 19:54:18 +0200 Subject: [PATCH] FlightTaskOrbit: use StraightLine library to approach circle The initial approach to the circle to orbit on was very agressive since it was just the controller trying to stay on the circle reaching the limits. Now there's first an approach phase in which the vehicle reaches the circle trajeectory in a smooth perpendicular line before starting the orbit execution. --- .../tasks/Orbit/FlightTaskOrbit.cpp | 55 +++++++++++++++---- .../tasks/Orbit/FlightTaskOrbit.hpp | 13 ++++- 2 files changed, 57 insertions(+), 11 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp index 2c496e56b1..38cdb007a3 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,7 +42,7 @@ using namespace matrix; -FlightTaskOrbit::FlightTaskOrbit() +FlightTaskOrbit::FlightTaskOrbit() : _circle_approach_line(_position) { _sticks_data_required = false; } @@ -178,17 +178,20 @@ bool FlightTaskOrbit::update() setRadius(r); setVelocity(v); - // xy velocity to go around in a circle Vector2f center_to_position = Vector2f(_position) - _center; - Vector2f velocity_xy(-center_to_position(1), center_to_position(0)); - velocity_xy = velocity_xy.unit_or_zero(); - velocity_xy *= _v; + const float distance_to_center = center_to_position.norm(); - // xy velocity adjustment to stay on the radius distance - velocity_xy += (_r - center_to_position.norm()) * center_to_position.unit_or_zero(); + // 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; + } - _velocity_setpoint(0) = velocity_xy(0); - _velocity_setpoint(1) = velocity_xy(1); + if (_in_circle_approach) { + generate_circle_approach_setpoints(); + + } else { + generate_circle_setpoints(center_to_position); + } // make vehicle front always point towards the center _yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F; @@ -200,3 +203,35 @@ bool FlightTaskOrbit::update() return true; } + +void FlightTaskOrbit::generate_circle_approach_setpoints() +{ + if (_circle_approach_line.isEndReached()) { + // calculate target point on circle and plan a line trajectory + Vector2f start_to_center = _center - Vector2f(_position); + Vector2f start_to_circle = (start_to_center.norm() - _r) * start_to_center.unit_or_zero(); + Vector2f closest_circle_point = Vector2f(_position) + start_to_circle; + Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2)); + _circle_approach_line.setLineFromTo(_position, target); + _circle_approach_line.setSpeed(_param_mpc_xy_cruise.get()); + } + + // follow the planned line and switch to orbiting once the circle is reached + _circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint); + _in_circle_approach = !_circle_approach_line.isEndReached(); +} + +void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position) +{ + // xy velocity to go around in a circle + Vector2f velocity_xy(-center_to_position(1), center_to_position(0)); + velocity_xy = velocity_xy.unit_or_zero(); + velocity_xy *= _v; + + // 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; +} diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp index e7688df98f..08df6a453f 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,6 +43,7 @@ #include "FlightTaskManualAltitudeSmooth.hpp" #include +#include class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth { @@ -86,10 +87,16 @@ protected: bool setVelocity(const float v); private: + void generate_circle_approach_setpoints(); /**< generates setpoints to smoothly reach the closest point on the circle when starting from far away */ + void generate_circle_setpoints(matrix::Vector2f center_to_position); /**< generates xy setpoints to orbit the vehicle */ + float _r = 0.f; /**< radius with which to orbit the target */ float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */ matrix::Vector2f _center; /**< local frame coordinates of the center point */ + bool _in_circle_approach = false; + StraightLine _circle_approach_line; + // TODO: create/use parameters for limits const float _radius_min = 1.f; const float _radius_max = 100.f; @@ -97,4 +104,8 @@ private: const float _acceleration_max = 2.f; orb_advert_t _orbit_status_pub = nullptr; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_xy_cruise /**< cruise speed for circle approach */ + ) };