ManualSmoothZ: move vel_sp_previous update into class

src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp

@@ -58,14 +58,10 @@ void FlightTaskManualAltitudeSmooth::_updateSetpoints()
 	/* Get yaw, thrust */
 	FlightTaskManualAltitude::_updateSetpoints();
 
-	/* Smooth velocity setpoint */
-	float vel_sp[2] = {_vel_sp_z, _vel_sp_prev_z};
-	_smoothing.smoothVelFromSticks(vel_sp, _deltatime);
-	_vel_sp_z = vel_sp[0];
+	/* Smooth velocity in z*/
+	_smoothing.smoothVelFromSticks(_vel_sp_z, _deltatime);
 
 	/* Check for altitude lock*/
 	_updateAltitudeLock();
 
-	/* Update previous velocity setpoint for next smoothing iteration */
-	_vel_sp_prev_z = _vel_sp_z;
 }

src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.cpp

@@ -14,7 +14,7 @@
  *    distribution.
  * 3. Neither the name PX4 nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
+ *    without spec{fic prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
@@ -41,7 +41,7 @@
 #include <float.h>
 
 ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) :
-	_vel(vel), _stick(stick)
+	_vel(vel), _stick(stick), _vel_sp_prev(vel)
 {
 	_acc_max_up_h = param_find("MPC_ACC_UP_MAX");
 	_acc_max_down_h = param_find("MPC_ACC_DOWN_MAX");
@@ -57,7 +57,7 @@ ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) :
  * 2.) accelerate */
 
 void
-ManualSmoothingZ::smoothVelFromSticks(float vel_sp[2], const float dt)
+ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt)
 {
 	updateParams();
 
@@ -65,6 +65,8 @@ ManualSmoothingZ::smoothVelFromSticks(float vel_sp[2], const float dt)
 
 	velocitySlewRate(vel_sp, dt);
 
+	_vel_sp_prev = vel_sp;
+
 }
 
 void
@@ -90,8 +92,11 @@ ManualSmoothingZ::setParams()
 }
 
 void
-ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
+ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
 {
+	/* Check for max acceleration */
+	setMaxAcceleration();
+
 	/* check if zero input stick */
 	const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON);
 
@@ -115,7 +120,7 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
 		 * is no delay present when user demands to brake
 		 */
 
-		vel_sp[1] = _vel;
+		_vel_sp_prev = _vel;
 
 	}
 
@@ -137,7 +142,7 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
 
 	case ManualIntentionZ::acceleration: {
 
-			_acc_state_dependent = (getMaxAcceleration(vel_sp) - _acc_max_down)
+			_acc_state_dependent = (getMaxAcceleration() - _acc_max_down)
 					       * fabsf(_stick) + _acc_max_down;
 			break;
 		}
@@ -146,46 +151,46 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
 	_intention = intention;
 }
 
-float
-ManualSmoothingZ::getMaxAcceleration(float vel_sp[2])
+void
+ManualSmoothingZ::setMaxAcceleration()
 {
 	/* Note: NED frame */
 
-	if (_stick < 0.0f) {
+	if (_stick < -FLT_EPSILON) {
 		/* accelerating upward */
-		return _acc_max_up;
+		_max_acceleration =  _acc_max_up;
 
-	} else if (_stick > 0.0f) {
+	} else if (_stick > FLT_EPSILON) {
 		/* accelerating downward */
-		return _acc_max_down;
+		_max_acceleration = _acc_max_down;
 
 	} else {
 
 		/* want to brake */
 
-		if (fabsf(vel_sp[0] - vel_sp[1]) < FLT_EPSILON) {
+		if (fabsf(_vel_sp_prev) < FLT_EPSILON) {
 			/* at rest */
-			return _acc_max_up;
+			_max_acceleration = _acc_max_up;
 
-		} else if (vel_sp[0] < 0.0f) {
+		} else if (_vel_sp_prev < 0.0f) {
 			/* braking downward */
-			return _acc_max_down;
+			_max_acceleration = _acc_max_down;
 
 		} else {
 			/* braking upward */
-			return _acc_max_up;
+			_max_acceleration = _acc_max_up;
 		}
 	}
 }
 
 void
-ManualSmoothingZ::velocitySlewRate(float vel_sp[2], const float dt)
+ManualSmoothingZ::velocitySlewRate(float &vel_sp, const float dt)
 {
 	/* limit vertical acceleration */
-	float acc = (vel_sp[0] - vel_sp[1]) / dt;
+	float acc = (vel_sp - _vel_sp_prev) / dt;
 	float max_acc = (acc < 0.0f) ? -_acc_state_dependent : _acc_state_dependent;
 
 	if (fabsf(acc) > fabsf(max_acc)) {
-		vel_sp[0] = max_acc * dt + vel_sp[1];
+		vel_sp = max_acc * dt + _vel_sp_prev;
 	}
 }

src/lib/FlightTasks/tasks/Utility/ManualSmoothingZ.hpp

@@ -60,11 +60,11 @@ public:
 	 * 		  vel_sp will contain smoothed current previous set-point.
 	 * @param dt: time delta in seconds
 	 */
-	void smoothVelFromSticks(float vel_sp[2], const float dt);
+	void smoothVelFromSticks(float &vel_sp, const float dt);
 
 
 	/* Getter methods */
-	float getMaxAcceleration(float vel_sp[2]);
+	float getMaxAcceleration() {return _max_acceleration;};
 	ManualIntentionZ getIntention() {return _intention;};
 
 	/* Overwrite methods:
@@ -89,6 +89,8 @@ private:
 	 * _acc_max_down <= _acc_state_dependent <= _acc_max_up
 	 */
 	float _acc_state_dependent{0.0f};
+	float _vel_sp_prev;
+	float _max_acceleration;
 
 	/* Params */
 	param_t _acc_max_up_h{PARAM_INVALID};
@@ -100,9 +102,10 @@ private:
 	int _parameter_sub{-1};
 
 	/* Helper methods */
-	void velocitySlewRate(float vel_sp[2], const float dt);
+	void velocitySlewRate(float &vel_sp, const float dt);
 	void setParams();
 	void updateParams();
-	void updateAcceleration(float vel_sp[2], const float dt);
+	void updateAcceleration(float &vel_sp, const float dt);
+	void setMaxAcceleration();
 
 };