FlightTaskOrbit: Direct orbit approach with slowdown at intersection point

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 &center, 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, {

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;