FlightTasks: added possibility to apply task parameters from the vehicle command

src/lib/FlightTasks/FlightTasks.cpp

@@ -27,13 +27,19 @@ bool FlightTasks::_updateCommand()
 	}
 
 	/* evaluate command */
-	//printf("YAY %d\n", int(command.param1));
-
 	uint8_t switch_result = switchTask(int(command.param1));
 	uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
 
+	/* switch succeeded */
 	if (!switch_result) {
 		cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+
+		/* if the correct task is running apply parameters to it and see if it rejects */
+		if (isAnyTaskActive()) {
+			if (!_current_task->applyCommandParameters(command)) {
+				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+			}
+		}
 	}
 
 	/* send back acknowledgment */

src/lib/FlightTasks/FlightTasks.hpp

@@ -109,7 +109,7 @@ public:
 	{
 		/* switch to the running task, nothing to do */
 		if (task_number == _current_task_index) {
-			return true;
+			return 0;
 		}
 
 		/* disable the old task if there is any */
@@ -128,6 +128,10 @@ public:
 			_current_task = new (&_task_union.orbit) FlightTaskOrbit(this, "ORB");
 			break;
 
+		case -1:
+			/* disable tasks is a success */
+			return 0;
+
 		default:
 			/* invalid task */
 			return -1;

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -46,6 +46,7 @@
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
 
 #include "../SubscriptionArray.hpp"
 
@@ -71,6 +72,12 @@ public:
 	 */
 	virtual bool activate();
 
+	/**
+	 * To be called to adopt parameters from an arrived vehicle command
+	 * @return true if accepted, false if declined
+	 */
+	virtual bool applyCommandParameters(vehicle_command_s command) = 0;
+
 	/**
 	 * Call before activate() or update()
 	 * to initialize time and input data

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -56,6 +56,8 @@ public:
 
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
 
+	bool applyCommandParameters(vehicle_command_s command) override { return false; };
+
 	bool updateInitialize() override;
 
 	bool update() override;

src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp

@@ -51,6 +51,21 @@ FlightTaskOrbit::FlightTaskOrbit(control::SuperBlock *parent, const char *name)
 	_sticks_data_required = false;
 }
 
+bool FlightTaskOrbit::applyCommandParameters(vehicle_command_s command)
+{
+	float &r = command.param3;
+	float &v = command.param4;
+
+	if (math::isInRange(r, 5.f, 50.f) &&
+	    fabs(v) < 10.f) {
+		_r = r;
+		_v = v;
+		return true;
+	}
+
+	return false;
+}
+
 bool FlightTaskOrbit::activate()
 {
 	bool ret = FlightTaskManual::activate();

src/lib/FlightTasks/tasks/FlightTaskOrbit.hpp

@@ -50,6 +50,8 @@ public:
 
 	virtual ~FlightTaskOrbit() = default;
 
+	bool applyCommandParameters(vehicle_command_s command) override;
+
 	bool activate() override;
 
 	bool update() override;

src/lib/mathlib/math/Limits.hpp

@@ -72,6 +72,12 @@ constexpr const _Tp &constrain(const _Tp &val, const _Tp &min_val, const _Tp &ma
 	return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
 }
 
+template<typename _Tp>
+inline constexpr bool isInRange(const _Tp &val, const _Tp &min_val, const _Tp &max_val)
+{
+	return (min_val <= val) && (val <= max_val);
+}
+
 template<typename T>
 constexpr T radians(const T degrees)
 {