FlightTasks: Introduce the empty setpoint to reset the setpoint for every loop iteration and return it in case of no task running

src/lib/FlightTasks/FlightTasks.cpp

@@ -15,6 +15,16 @@ bool FlightTasks::update()
 	return false;
 }
 
+const vehicle_local_position_setpoint_s &FlightTasks::getPositionSetpoint()
+{
+	if (isAnyTaskActive()) {
+		return _current_task->getPositionSetpoint();
+
+	} else {
+		return FlightTask::empty_setpoint;
+	}
+}
+
 int FlightTasks::switchTask(int task_number)
 {
 	/* switch to the running task, nothing to do */

src/lib/FlightTasks/FlightTasks.hpp

@@ -73,7 +73,7 @@ public:
 	 * Get the output data from the current task
 	 * Only call when task is active!
 	 */
-	const vehicle_local_position_setpoint_s &getPositionSetpoint() { return _current_task->getPositionSetpoint(); }
+	const vehicle_local_position_setpoint_s &getPositionSetpoint();
 
 	/**
 	 * Convenient operator to get the output data from the current task

src/lib/FlightTasks/tasks/FlightTask.cpp

@@ -2,6 +2,7 @@
 #include <mathlib/mathlib.h>
 
 constexpr uint64_t FlightTask::_timeout;
+constexpr vehicle_local_position_setpoint_s FlightTask::empty_setpoint;
 
 
 bool FlightTask::initializeSubscriptions(SubscriptionArray &subscription_array)
@@ -21,6 +22,7 @@ bool FlightTask::activate()
 
 bool FlightTask::updateInitialize()
 {
+	_resetSetpoint();
 	_time_stamp_current = hrt_absolute_time();
 	_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
 	_deltatime  = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;

src/lib/FlightTasks/tasks/FlightTask.hpp

@@ -56,7 +56,7 @@ class FlightTask : public control::Block
 public:
 	FlightTask(control::SuperBlock *parent, const char *name) :
 		Block(parent, name)
-	{ }
+	{ _resetSetpoint(); }
 
 	virtual ~FlightTask() = default;
 
@@ -99,6 +99,8 @@ public:
 		return _vehicle_local_position_setpoint;
 	}
 
+	static constexpr vehicle_local_position_setpoint_s empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN};
+
 protected:
 	/* Time abstraction */
 	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
@@ -134,5 +136,7 @@ private:
 
 	vehicle_local_position_setpoint_s _vehicle_local_position_setpoint; /**< Output position setpoint that every task has */
 
+	void _resetSetpoint() { _vehicle_local_position_setpoint = empty_setpoint; }
+
 	bool _evaluate_vehicle_position();
 };

src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp

@@ -53,8 +53,8 @@ FlightTaskOrbit::FlightTaskOrbit(control::SuperBlock *parent, const char *name)
 
 bool FlightTaskOrbit::applyCommandParameters(vehicle_command_s command)
 {
-	float &r = command.param3;
-	float &v = command.param4;
+	float &r = command.param3; /**< commanded radius */
+	float &v = command.param4; /**< commanded velocity */
 
 	if (math::isInRange(r, 5.f, 50.f) &&
 	    fabs(v) < 10.f) {