flight_tasks: pass previous setpoint as const reference

- perform setpoint checks in place to simplify and avoid copy

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp

@@ -48,7 +48,7 @@ FlightTaskAuto::FlightTaskAuto() :
 
 }
 
-bool FlightTaskAuto::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskAuto::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp

@@ -83,7 +83,7 @@ public:
 	FlightTaskAuto();
 
 	virtual ~FlightTaskAuto() = default;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool updateFinalize() override;
 

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

@@ -41,19 +41,30 @@
 
 using namespace matrix;
 
-bool FlightTaskAutoLineSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskAutoMapper::activate(last_setpoint);
 
-	checkSetpoints(last_setpoint);
-	const Vector3f vel_prev(last_setpoint.vx, last_setpoint.vy, last_setpoint.vz);
-	const Vector3f pos_prev(last_setpoint.x, last_setpoint.y, last_setpoint.z);
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
+	Vector3f accel_prev{last_setpoint.acceleration};
+
+	for (int i = 0; i < 3; i++) {
+		// If the position setpoint is unknown, set to the current postion
+		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
+
+		// If the velocity setpoint is unknown, set to the current velocity
+		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
+
+		// No acceleration estimate available, set to zero if the setpoint is NAN
+		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+	}
 
 	for (int i = 0; i < 3; ++i) {
-		_trajectory[i].reset(last_setpoint.acceleration[i], vel_prev(i), pos_prev(i));
+		_trajectory[i].reset(accel_prev(i), vel_prev(i), pos_prev(i));
 	}
 
-	_yaw_sp_prev = last_setpoint.yaw;
+	_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
 	_updateTrajConstraints();
 
 	return ret;
@@ -69,30 +80,6 @@ void FlightTaskAutoLineSmoothVel::reActivate()
 	_trajectory[2].reset(0.f, 0.7f, _position(2));
 }
 
-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
-	for (int i = 0; i < 3; i++) {
-		if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
-	}
-
-	if (!PX4_ISFINITE(setpoints.yaw)) { setpoints.yaw = _yaw; }
-}
-
 /**
  * EKF reset handling functions
  * Those functions are called by the base FlightTask in

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

@@ -49,13 +49,11 @@ public:
 	FlightTaskAutoLineSmoothVel() = default;
 	virtual ~FlightTaskAutoLineSmoothVel() = default;
 
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 
 protected:
 
-	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
-
 	/** Reset position or velocity setpoints in case of EKF reset event */
 	void _ekfResetHandlerPositionXY() override;
 	void _ekfResetHandlerVelocityXY() override;

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -44,7 +44,7 @@ FlightTaskAutoMapper::FlightTaskAutoMapper() :
 	_sticks(this)
 {};
 
-bool FlightTaskAutoMapper::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskAuto::activate(last_setpoint);
 	_reset();

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -48,7 +48,7 @@ class FlightTaskAutoMapper : public FlightTaskAuto
 public:
 	FlightTaskAutoMapper();
 	virtual ~FlightTaskAutoMapper() = default;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool update() override;
 
 protected:

src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.cpp

@@ -36,7 +36,7 @@
 
 #include "FlightTaskDescend.hpp"
 
-bool FlightTaskDescend::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskDescend::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	// stay level to minimize horizontal drift

src/lib/flight_tasks/tasks/Descend/FlightTaskDescend.hpp

@@ -46,7 +46,7 @@ public:
 	virtual ~FlightTaskDescend() = default;
 
 	bool update() override;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.cpp

@@ -36,7 +36,7 @@
 
 #include "FlightTaskFailsafe.hpp"
 
-bool FlightTaskFailsafe::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskFailsafe::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;

src/lib/flight_tasks/tasks/Failsafe/FlightTaskFailsafe.hpp

@@ -47,7 +47,7 @@ public:
 
 	virtual ~FlightTaskFailsafe() = default;
 	bool update() override;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 
 private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -10,7 +10,7 @@ const ekf_reset_counters_s FlightTask::zero_reset_counters = {};
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
 const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
 
-bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();

src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp

@@ -80,7 +80,7 @@ public:
 	 * @param last_setpoint last output of the previous task
 	 * @return true on success, false on error
 	 */
-	virtual bool activate(vehicle_local_position_setpoint_s last_setpoint);
+	virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint);
 
 	/**
 	 * Call this to reset an active Flight Task

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp

@@ -61,7 +61,7 @@ bool FlightTaskManualAltitude::updateInitialize()
 	return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
 }
 
-bool FlightTaskManualAltitude::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskManualAltitude::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_yaw_setpoint = NAN;

src/lib/flight_tasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -48,7 +48,7 @@ class FlightTaskManualAltitude : public FlightTask
 public:
 	FlightTaskManualAltitude();
 	virtual ~FlightTaskManualAltitude() = default;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
 

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

@@ -41,14 +41,22 @@
 
 using namespace matrix;
 
-bool FlightTaskManualAltitudeSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskManualAltitudeSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);
 
 	// Check if the previous FlightTask provided setpoints
-	checkSetpoints(last_setpoint);
 
-	_smoothing.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z);
+	// If the position setpoint is unknown, set to the current postion
+	float z_sp_last = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2);
+
+	// If the velocity setpoint is unknown, set to the current velocity
+	float vz_sp_last = PX4_ISFINITE(last_setpoint.vz) ? last_setpoint.vz : _velocity(2);
+
+	// No acceleration estimate available, set to zero if the setpoint is NAN
+	float az_sp_last = PX4_ISFINITE(last_setpoint.acceleration[2]) ? last_setpoint.acceleration[2] : 0.f;
+
+	_smoothing.reset(az_sp_last, vz_sp_last, z_sp_last);
 
 	return ret;
 }
@@ -60,18 +68,6 @@ void FlightTaskManualAltitudeSmoothVel::reActivate()
 	_smoothing.reset(0.f, 0.f, _position(2));
 }
 
-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.acceleration[2])) { setpoints.acceleration[2] = 0.f; }
-}
-
 void FlightTaskManualAltitudeSmoothVel::_ekfResetHandlerPositionZ()
 {
 	_smoothing.setCurrentPosition(_position(2));

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

@@ -48,7 +48,7 @@ public:
 	FlightTaskManualAltitudeSmoothVel() = default;
 	virtual ~FlightTaskManualAltitudeSmoothVel() = default;
 
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 
 protected:
@@ -65,8 +65,6 @@ protected:
 	)
 
 private:
-	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
-
 	void _updateTrajConstraints();
 	void _setOutputState();
 

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.cpp

@@ -56,7 +56,7 @@ bool FlightTaskManualPosition::updateInitialize()
 	       && PX4_ISFINITE(_velocity(1));
 }
 
-bool FlightTaskManualPosition::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskManualPosition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	// all requirements from altitude-mode still have to hold
 	bool ret = FlightTaskManualAltitude::activate(last_setpoint);

src/lib/flight_tasks/tasks/ManualPosition/FlightTaskManualPosition.hpp

@@ -49,7 +49,7 @@ public:
 	FlightTaskManualPosition();
 
 	virtual ~FlightTaskManualPosition() = default;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 
 	/**

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

@@ -38,18 +38,28 @@
 
 using namespace matrix;
 
-bool FlightTaskManualPositionSmoothVel::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskManualPositionSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualPosition::activate(last_setpoint);
 
 	// Check if the previous FlightTask provided setpoints
-	checkSetpoints(last_setpoint);
-	const Vector2f accel_prev(last_setpoint.acceleration[0], last_setpoint.acceleration[1]);
-	const Vector2f vel_prev(last_setpoint.vx, last_setpoint.vy);
-	const Vector2f pos_prev(last_setpoint.x, last_setpoint.y);
+	Vector3f accel_prev{last_setpoint.acceleration};
+	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
+	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
 
-	_smoothing_xy.reset(accel_prev, vel_prev, pos_prev);
-	_smoothing_z.reset(last_setpoint.acceleration[2], last_setpoint.vz, last_setpoint.z);
+	for (int i = 0; i < 3; i++) {
+		// If the position setpoint is unknown, set to the current postion
+		if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); }
+
+		// If the velocity setpoint is unknown, set to the current velocity
+		if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); }
+
+		// No acceleration estimate available, set to zero if the setpoint is NAN
+		if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; }
+	}
+
+	_smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev});
+	_smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2));
 
 	return ret;
 }
@@ -62,28 +72,6 @@ void FlightTaskManualPositionSmoothVel::reActivate()
 	_smoothing_z.reset(0.f, 0.f, _position(2));
 }
 
-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
-	for (int i = 0; i < 3; i++) {
-		if (!PX4_ISFINITE(setpoints.acceleration[i])) { setpoints.acceleration[i] = 0.f; }
-	}
-}
-
 void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY()
 {
 	_smoothing_xy.setCurrentPosition(_position.xy());

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

@@ -53,7 +53,7 @@ public:
 
 	virtual ~FlightTaskManualPositionSmoothVel() = default;
 
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	void reActivate() override;
 
 protected:
@@ -73,8 +73,6 @@ protected:
 				       )
 
 private:
-	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
-
 	void _updateTrajConstraints();
 	void _updateTrajConstraintsXY();
 	void _updateTrajConstraintsZ();

src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp

@@ -57,7 +57,7 @@ bool FlightTaskOffboard::updateInitialize()
 	       && PX4_ISFINITE(_velocity(1));
 }
 
-bool FlightTaskOffboard::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskOffboard::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTask::activate(last_setpoint);
 	_position_setpoint = _position;

src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.hpp

@@ -48,7 +48,7 @@ public:
 
 	virtual ~FlightTaskOffboard() = default;
 	bool update() override;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 
 protected:

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -154,7 +154,7 @@ bool FlightTaskOrbit::checkAcceleration(float r, float v, float a)
 	return v * v < a * r;
 }
 
-bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
 	bool ret = FlightTaskManualAltitudeSmooth::activate(last_setpoint);
 	_r = _radius_min;

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp

@@ -53,7 +53,7 @@ public:
 	virtual ~FlightTaskOrbit() = default;
 
 	bool applyCommandParameters(const vehicle_command_s &command) override;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool update() override;
 
 	/**

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

@@ -42,22 +42,13 @@ bool FlightTaskTransition::updateInitialize()
 	return FlightTask::updateInitialize();
 }
 
-bool FlightTaskTransition::activate(vehicle_local_position_setpoint_s last_setpoint)
+bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
-	checkSetpoints(last_setpoint);
-	_transition_altitude = last_setpoint.z;
-	_transition_yaw = last_setpoint.yaw;
+	_transition_altitude = PX4_ISFINITE(last_setpoint.z) ? last_setpoint.z : _position(2);
+	_transition_yaw = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;
 	return FlightTask::activate(last_setpoint);
 }
 
-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; }
-}
-
 bool FlightTaskTransition::update()
 {
 	bool ret = FlightTask::update();

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

@@ -47,13 +47,11 @@ public:
 	FlightTaskTransition() = default;
 
 	virtual ~FlightTaskTransition() = default;
-	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
+	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
 
 private:
-	void checkSetpoints(vehicle_local_position_setpoint_s &setpoints);
-
 	float _transition_altitude = 0.0f;
 	float _transition_yaw = 0.0f;
 };