FlightTask: fail activation if required states are not available

src/lib/FlightTasks/tasks/FlightTaskAuto.cpp

@@ -63,6 +63,14 @@ bool FlightTaskAuto::activate()
 	_prev_prev_wp = _prev_wp = _target = _next_wp = _position;
 	_setDefaultConstraints();
 
+	// need a valid position and velocity
+	ret = ret && PX4_ISFINITE(_position(0))
+	      && PX4_ISFINITE(_position(1))
+	      && PX4_ISFINITE(_position(2))
+	      && PX4_ISFINITE(_velocity(0))
+	      && PX4_ISFINITE(_velocity(1))
+	      && PX4_ISFINITE(_velocity(2));
+
 	return ret;
 }
 

src/lib/FlightTasks/tasks/FlightTaskManual.cpp

@@ -65,6 +65,21 @@ bool FlightTaskManual::updateInitialize()
 	return ret;
 }
 
+bool FlightTaskManual::activate()
+{
+	bool ret = FlightTask::activate();
+
+	if (_sticks_data_required) {
+		// need valid stick inputs
+		ret = ret && PX4_ISFINITE(_sticks(0))
+		      && PX4_ISFINITE(_sticks(1))
+		      && PX4_ISFINITE(_sticks(2))
+		      && PX4_ISFINITE(_sticks(3));
+	}
+
+	return ret;
+}
+
 bool FlightTaskManual::_evaluateSticks()
 {
 	/* Sticks are rescaled linearly and exponentially to [-1,1] */

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -51,10 +51,10 @@ public:
 	virtual ~FlightTaskManual() = default;
 
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
-
 	bool applyCommandParameters(const vehicle_command_s &command) override { return FlightTask::applyCommandParameters(command); };
-
 	bool updateInitialize() override;
+	bool activate() override;
+
 
 protected:
 

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

@@ -47,6 +47,9 @@ bool FlightTaskManualAltitude::activate()
 	_position_setpoint(2) = _position(2);
 	_velocity_setpoint(2) = 0.0f;
 	_setDefaultConstraints();
+
+	// altitude-mode requires to have a valid position and velocity state in D-direction
+	ret = ret &&  PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2));
 	return ret;
 }
 

src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp

@@ -44,6 +44,7 @@ using namespace matrix;
 bool FlightTaskManualPosition::activate()
 {
 
+	// all requirements from altitude-mode still have to hold
 	bool ret = FlightTaskManualAltitude::activate();
 
 	// set task specific constraint
@@ -55,6 +56,13 @@ bool FlightTaskManualPosition::activate()
 	_position_setpoint(1) = _position(1);
 	_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
 	_velocity_scale = _constraints.speed_xy;
+
+	// for position-controlled mode, we need a valid position and velocity state
+	// in NE-direction
+	ret = ret && PX4_ISFINITE(_position(0))
+	      && PX4_ISFINITE(_position(1))
+	      && PX4_ISFINITE(_velocity(0))
+	      && PX4_ISFINITE(_velocity(1));
 	return ret;
 }
 

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp

@@ -62,6 +62,10 @@ bool FlightTaskManualStabilized::activate()
 	_yaw_setpoint = _yaw;
 	_yawspeed_setpoint = 0.0f;
 	_constraints.tilt = math::radians(_tilt_max_man.get());
+
+	// need a valid yaw
+	ret = ret && PX4_ISFINITE(_yaw);
+
 	return ret;
 }
 

src/lib/FlightTasks/tasks/FlightTaskOffboard.cpp

@@ -58,6 +58,15 @@ bool FlightTaskOffboard::activate()
 	_position_setpoint = _position;
 	_velocity_setpoint *= 0.0f;
 	_position_lock *= NAN;
+
+	// need a valid position and velocity
+	ret = ret && PX4_ISFINITE(_position(0))
+	      && PX4_ISFINITE(_position(1))
+	      && PX4_ISFINITE(_position(2))
+	      && PX4_ISFINITE(_velocity(0))
+	      && PX4_ISFINITE(_velocity(1))
+	      && PX4_ISFINITE(_velocity(2));
+
 	return ret;
 }
 

src/lib/FlightTasks/tasks/FlightTaskOrbit.cpp

@@ -67,6 +67,15 @@ bool FlightTaskOrbit::activate()
 	_z = _position(2);
 	_center = Vector2f(_position.data());
 	_center(0) -= _r;
+
+	// need a valid position and velocity
+	ret = ret && PX4_ISFINITE(_position(0))
+	      && PX4_ISFINITE(_position(1))
+	      && PX4_ISFINITE(_position(2))
+	      && PX4_ISFINITE(_velocity(0))
+	      && PX4_ISFINITE(_velocity(1))
+	      && PX4_ISFINITE(_velocity(2));
+
 	return ret;
 }