From d450afead6bb5c9abf6b3971621d3ad03c6d6561 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Thu, 7 Oct 2021 16:54:06 +0200 Subject: [PATCH] FlightTaskOrbit: Direct orbit approach with slowdown at intersection point --- .../tasks/Orbit/FlightTaskOrbit.cpp | 31 +------------------ .../tasks/Orbit/FlightTaskOrbit.hpp | 4 +-- 2 files changed, 3 insertions(+), 32 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 9c40964456..cb994ba136 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -280,26 +280,6 @@ bool FlightTaskOrbit::_is_position_on_circle() const } -bool circle_tangents_for_position(const Vector2f ¢er, float radius, const Vector2f &position, Vector2f &out1, - Vector2f &out2) -{ - const Vector2f d = position - center; - const Vector2f dr = {-d(1), d(0)}; - float d_norm = d.length(); - - if (d_norm >= radius) { - float rho = radius / d_norm; - float ad = rho * rho; - float bd = rho * sqrtf(1.f - ad); - out1 = center + ad * d + bd * dr; - out2 = center + ad * d - bd * dr; - return true; - } - - return false; -} - - void FlightTaskOrbit::_generate_circle_approach_setpoints() { const Vector2f center2d = Vector2f(_center); @@ -307,16 +287,7 @@ void FlightTaskOrbit::_generate_circle_approach_setpoints() Vector2f closest_point_on_circle = Vector2f(_position) + position_to_center_xy.unit_or_zero() * (position_to_center_xy.norm() - _orbit_radius); - float angle = math::radians(8.f); - float s_a = _orbit_velocity >= 0 ? sinf(angle) : -sinf(angle); - float c_a = cosf(angle); - Vector2f origin_closest = (closest_point_on_circle - center2d); - Vector2f target_circle_point_xy = { - center2d(0) + c_a * origin_closest(0) - s_a * origin_closest(1), - center2d(1) + s_a * origin_closest(0) + c_a * origin_closest(1) - }; - - const Vector3f target_circle_point{target_circle_point_xy(0), target_circle_point_xy(1), _center(2)}; + const Vector3f target_circle_point{closest_point_on_circle(0), closest_point_on_circle(1), _center(2)}; PositionSmoothing::PositionSmoothingSetpoints out_setpoints; _position_smoothing.generateSetpoints(_position, { diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index e68902f24f..fdc882f4a2 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -114,8 +114,8 @@ private: /** generates yaw setpoints to control the vehicle's heading */ void _generate_circle_yaw_setpoints(); - float _orbit_velocity; - float _orbit_radius; + float _orbit_velocity{}; + float _orbit_radius{}; matrix::Vector3f _center; /**< local frame coordinates of the center point */ bool _in_circle_approach = false;