From e8d2f1c2bee69788f4d1e33b9b868ff73a23e187 Mon Sep 17 00:00:00 2001
From: MaEtUgR <maetugr@gmail.com>
Date: Thu, 12 Jul 2018 11:50:48 +0200
Subject: [PATCH] FlightTaskOrbit: execute MAVLink orbit command

Latest QGC daily supports sending an orbit command. This commit applies
the correct radius, rotation direction, velocity and center position.
Still missing is a check to have global position available and applying
x,y coordinates and altitude independently.
---
 src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp | 42 ++++++++++++++++---
 src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp |  2 +-
 2 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
index 7129af4218..ae713cb3c1 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp
@@ -37,6 +37,7 @@
 
 #include "FlightTaskOrbit.hpp"
 #include <mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
 
 using namespace matrix;
 
@@ -47,14 +48,43 @@ FlightTaskOrbit::FlightTaskOrbit()
 
 bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 {
-	const float &r = command.param3; // commanded radius
-	const float &v = command.param4; // commanded velocity
+	bool ret = true;
+	bool clockwise = _v > 0;
 
-	if (setRadius(r) && setVelocity(v)) {
-		return FlightTaskManualAltitudeSmooth::applyCommandParameters(command);
+	// commanded radius
+	if (PX4_ISFINITE(command.param1)) {
+		clockwise = command.param1 > 0;
+		const float r = fabsf(command.param1);
+		ret = ret && setRadius(r);
 	}
 
-	return false;
+	// commanded velocity, take sign of radius as rotation direction
+	if (PX4_ISFINITE(command.param2)) {
+		const float v = command.param2 * (clockwise ? 1.f : -1.f);
+		ret = ret && setVelocity(v);
+	}
+
+	// TODO: apply x,y / z independently in geo library
+	// commanded center coordinates
+	// if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
+	// 	map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1));
+	// }
+
+	// commanded altitude
+	// if(PX4_ISFINITE(command.param7)) {
+	// 	_position_setpoint(2) = gl_ref.alt - command.param7;
+	// }
+
+	if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6) && PX4_ISFINITE(command.param7)) {
+		if(globallocalconverter_tolocal(command.param5, command.param6, command.param7, &_center(0), &_center(1),
+					     &_position_setpoint(2)))
+		{
+			// global to local conversion failed
+			ret = false;
+		}
+	}
+
+	return ret;
 }
 
 bool FlightTaskOrbit::setRadius(const float r)
@@ -92,7 +122,7 @@ bool FlightTaskOrbit::activate()
 {
 	bool ret = FlightTaskManualAltitudeSmooth::activate();
 	_r = _radius_min;
-	_v =  0.5f;
+	_v =  1.f;
 	_center = Vector2f(_position.data());
 	_center(0) -= _r;
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
index d40c941372..24f2ce208a 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp
@@ -86,7 +86,7 @@ private:
 
 	// TODO: create/use parameters for limits
 	const float _radius_min = 1.f;
-	const float _radius_max = 20.f;
+	const float _radius_max = 100.f;
 	const float _velocity_max = 10.f;
 	const float _acceleration_max = 2.f;
 };