FlightTask - Move checkSetpoints function in each FlightTask to be tailored to the specific needs and avoid checking all the setpoints

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -54,7 +54,7 @@ bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s sta
 		_trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
 	}
 
-	_yaw_sp_prev = _yaw;
+	_yaw_sp_prev = state_prev.yaw;
 	_updateTrajConstraints();
 	_initEkfResetCounters();
 
@@ -72,6 +72,32 @@ void FlightTaskAutoLineSmoothVel::reActivate()
 	_initEkfResetCounters();
 }
 
+void FlightTaskAutoLineSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the position setpoint is unknown, set to the current postion
+	if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); }
+
+	if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); }
+
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); }
+
+	if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); }
+
+	if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
+}
+
 void FlightTaskAutoLineSmoothVel::_generateSetpoints()
 {
 	_prepareSetpoints();

src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -64,6 +64,7 @@ protected:
 					(ParamFloat<px4::params::MPC_Z_TRAJ_P>) _param_mpc_z_traj_p
 				       );
 
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
 	void _generateSetpoints() override; /**< Generate setpoints along line. */
 
 	/** determines when to trigger a takeoff (ignored in flight) */

src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp

@@ -47,47 +47,6 @@ bool FlightTask::updateInitialize()
 	return true;
 }
 
-void FlightTask::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
-{
-	// If the position setpoint is unknown, set to the current postion
-	if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); }
-
-	if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); }
-
-	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
-
-	// If the velocity setpoint is unknown, set to the current velocity
-	if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); }
-
-	if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); }
-
-	if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
-
-	// No acceleration estimate available, set to zero if the setpoint is NAN
-	if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; }
-
-	if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; }
-
-	if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
-
-	// No jerk estimate available, set to zero if the setpoint is NAN
-	if (!PX4_ISFINITE(setpoints.jerk_x)) { setpoints.jerk_x = 0.f; }
-
-	if (!PX4_ISFINITE(setpoints.jerk_y)) { setpoints.jerk_y = 0.f; }
-
-	if (!PX4_ISFINITE(setpoints.jerk_z)) { setpoints.jerk_z = 0.f; }
-
-	if (!PX4_ISFINITE(setpoints.thrust[0])) { setpoints.thrust[0] = 0.f; }
-
-	if (!PX4_ISFINITE(setpoints.thrust[1])) { setpoints.thrust[1] = 0.f; }
-
-	if (!PX4_ISFINITE(setpoints.thrust[2])) { setpoints.thrust[2] = -0.5f; }
-
-	if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
-
-	if (!PX4_ISFINITE(setpoints.yawspeed)) { setpoints.yawspeed = 0.f; }
-}
-
 const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
 {
 	/* fill position setpoint message */

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -120,8 +120,6 @@ public:
 	 */
 	const vehicle_local_position_setpoint_s getPositionSetpoint();
 
-	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
-
 	/**
 	 * Get vehicle constraints.
 	 * The constraints can vary with task.

src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp

@@ -66,6 +66,18 @@ void FlightTaskManualAltitudeSmoothVel::reActivate()
 	_resetPositionLock();
 }
 
+void FlightTaskManualAltitudeSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the position setpoint is unknown, set to the current postion
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
+}
+
 void FlightTaskManualAltitudeSmoothVel::_resetPositionLock()
 {
 	// Always start unlocked

src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp

@@ -63,6 +63,7 @@ protected:
 
 private:
 
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
 	void _resetPositionLock();
 	void _initEkfResetCounters();
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */

src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp

@@ -72,6 +72,30 @@ void FlightTaskManualPositionSmoothVel::reActivate()
 	_resetPositionLock();
 }
 
+void FlightTaskManualPositionSmoothVel::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the position setpoint is unknown, set to the current postion
+	if (!PX4_ISFINITE(setpoints.x)) { setpoints.x = _position(0); }
+
+	if (!PX4_ISFINITE(setpoints.y)) { setpoints.y = _position(1); }
+
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	if (!PX4_ISFINITE(setpoints.vx)) { setpoints.vx = _velocity(0); }
+
+	if (!PX4_ISFINITE(setpoints.vy)) { setpoints.vy = _velocity(1); }
+
+	if (!PX4_ISFINITE(setpoints.vz)) { setpoints.vz = _velocity(2); }
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	if (!PX4_ISFINITE(setpoints.acc_x)) { setpoints.acc_x = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.acc_y)) { setpoints.acc_y = 0.f; }
+
+	if (!PX4_ISFINITE(setpoints.acc_z)) { setpoints.acc_z = 0.f; }
+}
+
 void FlightTaskManualPositionSmoothVel::_resetPositionLock()
 {
 	_position_lock_xy_active = false;

src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp

@@ -62,6 +62,7 @@ protected:
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
 				       )
 private:
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
 	void _resetPositionLock();
 	void _initEkfResetCounters();
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */

src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.cpp

@@ -52,6 +52,14 @@ bool FlightTaskTransition::activate(vehicle_local_position_setpoint_s state_prev
 	return FlightTask::activate(state_prev);
 }
 
+void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &setpoints)
+{
+	// If the setpoint is unknown, set to the current estimate
+	if (!PX4_ISFINITE(setpoints.z)) { setpoints.z = _position(2); }
+
+	if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
+}
+
 void FlightTaskTransition::updateAccelerationEstimate()
 {
 	// Estimate the acceleration by filtering the raw derivative of the velocity estimate

src/lib/FlightTasks/tasks/Transition/FlightTaskTransition.hpp

@@ -52,6 +52,7 @@ public:
 	bool update() override;
 
 private:
+	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
 	void updateAccelerationEstimate();
 
 	float _transition_altitude = 0.0f;