FlightTasks: adapt tasks to member setpoints

The FlightTaskManual... tasks already had their internal
setpoint member variables. I switched them to use the
architecture with setpoint member variables as it was
implemented the commit before. They simplify a lot.

src/lib/FlightTasks/tasks/FlightTaskManual.cpp

@@ -98,12 +98,3 @@ bool FlightTaskManual::_evaluateSticks()
 		return false;
 	}
 }
-
-void FlightTaskManual::_resetToNAN()
-{
-	_thr_sp *= NAN;
-	_vel_sp *= NAN;
-	_pos_sp *= NAN;
-	_yaw_sp = NAN;
-	_yaw_rate_sp = NAN;
-}

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -63,15 +63,6 @@ protected:
 	matrix::Vector3f _sticks_expo; /**< modified manual sticks using expo function*/
 	control::BlockParamFloat _stick_dz; /**< 0-deadzone around the center for the sticks */
 
-	/* Setpoints: NAN means that setpoint is not being considered. */
-	matrix::Vector3f _thr_sp{NAN, NAN, NAN}; /**< thrust setpoint */
-	matrix::Vector3f _vel_sp{NAN, NAN, NAN}; /**< velocity setpoint */
-	matrix::Vector3f _pos_sp{NAN, NAN, NAN}; /**< position setpoint */
-	float _yaw_sp{NAN};						 /**< yaw setpoint */
-	float _yaw_rate_sp{NAN};				 /**< yawspeed setpoint */
-
-	void _resetToNAN();
-
 private:
 
 	uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp

@@ -48,13 +48,6 @@ FlightTaskManualAltitude::FlightTaskManualAltitude(control::SuperBlock *parent,
 
 {}
 
-bool FlightTaskManualAltitude::activate()
-{
-	bool ret = FlightTaskManualStabilized::activate();
-	_vel_sp(2) = 0.0f;
-	return  ret;
-}
-
 void FlightTaskManualAltitude::_scaleSticks()
 {
 	/* Reuse same scaling as for stabilized */
@@ -62,7 +55,7 @@ void FlightTaskManualAltitude::_scaleSticks()
 
 	/* Scale horizontal velocity with expo curve stick input*/
 	const float vel_max_z = (_sticks(2) > 0.0f) ? _vel_max_down.get() : _vel_max_up.get();
-	_vel_sp(2) = vel_max_z * _sticks_expo(2);
+	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
 }
 
 void FlightTaskManualAltitude::_updateAltitudeLock()
@@ -72,14 +65,14 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 	 */
 
 	/* handle position and altitude hold */
-	const bool apply_brake_z = fabsf(_vel_sp(2)) <= FLT_EPSILON;
+	const bool apply_brake_z = fabsf(_velocity_setpoint(2)) <= FLT_EPSILON;
 	const bool stopped_z = (_vel_hold_thr_z.get() < FLT_EPSILON || fabsf(_velocity(2)) < _vel_hold_thr_z.get());
 
-	if (apply_brake_z && stopped_z && !PX4_ISFINITE(_pos_sp(2))) {
+	if (apply_brake_z && stopped_z && !PX4_ISFINITE(_position_setpoint(2))) {
-		_pos_sp(2) = _position(2);
+		_position_setpoint(2) = _position(2);
 
 	} else if (!apply_brake_z) {
-		_pos_sp(2) = NAN;
+		_position_setpoint(2) = NAN;
 	}
 }
 
@@ -87,7 +80,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 {
 	FlightTaskManualStabilized::_updateSetpoints(); // get yaw and thrust setpoints
 
-	_thr_sp *= NAN; // Don't need thrust setpoint from Stabilized mode.
+	_thrust_setpoint *= NAN; // Don't need thrust setpoint from Stabilized mode.
 
 	/* Thrust in xy are extracted directly from stick inputs. A magnitude of
 	 * 1 means that maximum thrust along xy is required. A magnitude of 0 means no
@@ -101,8 +94,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
 		sp.normalize();
 	}
 
-	_thr_sp(0) = sp(0);
+	_thrust_setpoint(0) = sp(0);
-	_thr_sp(1) = sp(1);
+	_thrust_setpoint(1) = sp(1);
 
 	_updateAltitudeLock();
 }

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp

@@ -48,8 +48,6 @@ public:
 
 	virtual ~FlightTaskManualAltitude() = default;
 
-	bool activate() override;
-
 protected:
 	control::BlockParamFloat _vel_max_down; /**< maximum speed allowed to go up */
 	control::BlockParamFloat _vel_max_up; /**< maximum speed allowed to go down */

src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp

@@ -50,7 +50,7 @@ void FlightTaskManualAltitudeSmooth::_updateSetpoints()
 	FlightTaskManualAltitude::_updateSetpoints();
 
 	/* Smooth velocity in z*/
-	_smoothing.smoothVelFromSticks(_vel_sp(2), _deltatime);
+	_smoothing.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
 
 	/* Check for altitude lock*/
 	_updateAltitudeLock();

src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp

@@ -48,13 +48,6 @@ FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent,
 	_vel_hold_thr_xy(parent,  "MPC_HOLD_MAX_XY", false)
 {}
 
-bool FlightTaskManualPosition::activate()
-{
-	bool ret = FlightTaskManualAltitude::activate();
-	_vel_sp(0) = _vel_sp(1) = 0.0f;
-	return ret;
-}
-
 void FlightTaskManualPosition::_scaleSticks()
 {
 	/* Use same scaling as for FlightTaskManualAltitude */
@@ -74,31 +67,31 @@ void FlightTaskManualPosition::_scaleSticks()
 
 	/* Rotate setpoint into local frame. */
 	_rotateIntoHeadingFrame(vel_sp_xy);
-	_vel_sp(0) = vel_sp_xy(0);
+	_velocity_setpoint(0) = vel_sp_xy(0);
-	_vel_sp(1) = vel_sp_xy(1);
+	_velocity_setpoint(1) = vel_sp_xy(1);
 }
 
 void FlightTaskManualPosition::_updateXYlock()
 {
 	/* If position lock is not active, position setpoint is set to NAN.*/
 	const float vel_xy_norm = Vector2f(&_velocity(0)).length();
-	const bool apply_brake = Vector2f(&_vel_sp(0)).length() < FLT_EPSILON;
+	const bool apply_brake = Vector2f(&_velocity_setpoint(0)).length() < FLT_EPSILON;
 	const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());
 
-	if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp(0))) {
+	if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
-		_pos_sp(0) = _position(0);
+		_position_setpoint(0) = _position(0);
-		_pos_sp(1) = _position(1);
+		_position_setpoint(1) = _position(1);
 
 	} else if (!apply_brake) {
 		/* don't lock*/
-		_pos_sp(0) = NAN;
+		_position_setpoint(0) = NAN;
-		_pos_sp(1) = NAN;
+		_position_setpoint(1) = NAN;
 	}
 }
 
 void FlightTaskManualPosition::_updateSetpoints()
 {
 	FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
-	_thr_sp *= NAN; // don't require any thrust setpoints
+	_thrust_setpoint *= NAN; // don't require any thrust setpoints
 	_updateXYlock(); // check for position lock
 }

src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp

@@ -49,8 +49,6 @@ public:
 
 	virtual ~FlightTaskManualPosition() = default;
 
-	bool activate() override;
-
 protected:
 	control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */
 	control::BlockParamFloat _acc_xy_max;/**< maximum acceleration horizontally. Only used to compute lock time */

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp

@@ -52,16 +52,16 @@ void FlightTaskManualPositionSmooth::_updateSetpoints()
 
 	/* Smooth velocity setpoint in xy.*/
 	matrix::Vector2f vel(&_velocity(0));
-	Vector2f vel_sp_xy = Vector2f(&_vel_sp(0));
+	Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
-	_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yaw_rate_sp, _deltatime);
+	_smoothingXY.smoothVelocity(vel_sp_xy, vel, _yaw, _yawspeed_setpoint, _deltatime);
-	_vel_sp(0) = vel_sp_xy(0);
+	_velocity_setpoint(0) = vel_sp_xy(0);
-	_vel_sp(1) = vel_sp_xy(1);
+	_velocity_setpoint(1) = vel_sp_xy(1);
 
 	/* Check for altitude lock.*/
 	_updateXYlock();
 
 	/* Smooth velocity in z.*/
-	_smoothingZ.smoothVelFromSticks(_vel_sp(2), _deltatime);
+	_smoothingZ.smoothVelFromSticks(_velocity_setpoint(2), _deltatime);
 
 	/* Check for altitude lock*/
 	_updateAltitudeLock();

src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp

@@ -52,9 +52,7 @@ FlightTaskManualStabilized::FlightTaskManualStabilized(control::SuperBlock *pare
 
 bool FlightTaskManualStabilized::activate()
 {
-	_resetToNAN();
+	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
-	_yaw_rate_sp = 0.0f;
-	_thr_sp = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
 	return FlightTaskManual::activate();
 }
 
@@ -62,8 +60,9 @@ void FlightTaskManualStabilized::_scaleSticks()
 {
 	/* Scale sticks to yaw and thrust using
 	 * linear scale for yaw and piecewise linear map for thrust. */
-	_yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get());
+	_yawspeed_setpoint = _sticks(3) * math::radians(_yaw_rate_scaling.get());
-	_yaw_rate_sp = math::sign(_yaw_rate_sp) * math::min(fabsf(_yaw_rate_sp), math::radians(_yaw_rate_max.get()));
+	_yawspeed_setpoint = math::sign(_yawspeed_setpoint) * math::min(fabsf(_yawspeed_setpoint),
+			     math::radians(_yaw_rate_max.get()));
 	_throttle = _throttleCurve();
 }
 
@@ -74,11 +73,11 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
 	 * TODO: add yawspeed to get threshold.*/
 	const bool stick_yaw_zero = fabsf(_sticks(3)) <= _stick_dz.get();
 
-	if (stick_yaw_zero && !PX4_ISFINITE(_yaw_sp)) {
+	if (stick_yaw_zero && !PX4_ISFINITE(_yaw_setpoint)) {
-		_yaw_sp = _yaw;
+		_yaw_setpoint = _yaw;
 
 	} else if (!stick_yaw_zero) {
-		_yaw_sp = NAN;
+		_yaw_setpoint = NAN;
 	}
 }
 
@@ -111,12 +110,12 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
 	 * upward by the Axis-Angle.
 	 */
 	Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f);
-	_thr_sp = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
+	_thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
 }
 
 void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v)
 {
-	float yaw_rotate = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw;
+	float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
 	Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
 	v(0) = v_r(0);
 	v(1) = v_r(1);
@@ -147,11 +146,5 @@ bool FlightTaskManualStabilized::update()
 	_scaleSticks();
 	_updateSetpoints();
 
-	_position_setpoint = _pos_sp;
-	_velocity_setpoint = _vel_sp;
-	_thrust_setpoint = _thr_sp;
-	_yaw_setpoint = _yaw_sp;
-	_yawspeed_setpoint = _yaw_rate_sp;
-
 	return true;
 }

src/lib/FlightTasks/tasks/FlightTaskSport.hpp

@@ -61,11 +61,11 @@ protected:
 		FlightTaskManualPosition::_updateSetpoints(); // get all setpoints from position task
 
 		/* Scale horizontal velocity setpoint by maximum allowed velocity. */
-		if (PX4_ISFINITE(_vel_sp(0)) && Vector2f(&_vel_sp(0)).length() > 0.0f) {
+		if (PX4_ISFINITE(_velocity_setpoint(0)) && Vector2f(&_velocity_setpoint(0)).length() > 0.0f) {
-			Vector2f vel_sp_xy = Vector2f(&_vel_sp(0));
+			Vector2f vel_sp_xy = Vector2f(&_velocity_setpoint(0));
 			vel_sp_xy = vel_sp_xy.normalized() * _vel_xy_max.get() / _vel_xy_manual_max.get() * vel_sp_xy.length();
-			_vel_sp(0) = vel_sp_xy(0);
+			_velocity_setpoint(0) = vel_sp_xy(0);
-			_vel_sp(1) = vel_sp_xy(1);
+			_velocity_setpoint(1) = vel_sp_xy(1);
 		}
 	}