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.

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;
+}

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 <uORB/uORB.h>
+#include <StraightLine.hpp>
 
 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<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise /**< cruise speed for circle approach */
+	)
 };