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();
 
 };

src/systemcmds/tests/test_smooth_z.cpp

@@ -34,7 +34,6 @@ bool SmoothZTest::brakeUpward()
 	float acc_max_up = 5.0f;
 	float acc_max_down = 2.0f;
 
-	float vel_sp[] = {vel_sp_current, vel_sp_previous};
 	ManualSmoothingZ smooth(vel, stick_current);
 
 	/* overwrite parameters since they might change depending on configuration */
@@ -49,17 +48,17 @@ bool SmoothZTest::brakeUpward()
 
 	for (int i = 0; i < 100; i++) {
 
-		smooth.smoothVelFromSticks(vel_sp, dt);
+		smooth.smoothVelFromSticks(vel_sp_current, dt);
 
 		/* Test if intention is brake */
 		ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);
 
 		/* we should always use upward acceleration */
-		ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON));
+		ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON));
 
 		/* New setpoint has to be lower than previous setpoint (NED frame) or equal 0. 0 velocity
 		 * occurs once the vehicle is at perfect rest. */
-		ut_assert_true((vel_sp[0] < vel_sp[1]) || (fabsf(vel_sp[0]) < FLT_EPSILON));
+		ut_assert_true((vel_sp_current < vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));
 
 
 		/* We reset the previou setpoint to newest setpoint
@@ -67,9 +66,10 @@ bool SmoothZTest::brakeUpward()
 		 * We also set vel to previous setpoint where we make the assumption that
 		 * the vehicle can perfectly track the setpoints.
 		 */
-		vel_sp[1] = vel_sp[0];
-		vel_sp[0] = 0.0f;
-		vel = vel_sp[1];
+		vel_sp_previous = vel_sp_current;
+		vel_sp_current = 0.0f;
+		vel = vel_sp_previous;
+
 
 	}
 
@@ -86,7 +86,6 @@ bool SmoothZTest::brakeDownward()
 	float acc_max_up = 5.0f;
 	float acc_max_down = 2.0f;
 
-	float vel_sp[] = { vel_sp_current, vel_sp_previous };
 	ManualSmoothingZ smooth(vel, stick_current);
 
 	/* overwrite parameters since they might change depending on configuration */
@@ -101,21 +100,21 @@ bool SmoothZTest::brakeDownward()
 
 	for (int i = 0; i < 100; i++) {
 
-		smooth.smoothVelFromSticks(vel_sp, dt);
+		smooth.smoothVelFromSticks(vel_sp_current, dt);
 
 		/* Test if intention is brake */
 		ut_assert_true(smooth.getIntention() == ManualIntentionZ::brake);
 
 		/* New setpoint has to be larger than previous setpoint (NED frame) or equal 0. 0 velocity
 		 * occurs once the vehicle is at perfect rest. */
-		ut_assert_true((vel_sp[0] > vel_sp[1]) || (fabsf(vel_sp[0]) < FLT_EPSILON));
+		ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current) < FLT_EPSILON));
 
 		/* we should always use downward acceleration except when vehicle is at rest*/
-		if (fabsf(vel_sp[0]) < FLT_EPSILON) {
-			ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON));
+		if (fabsf(vel_sp_current) < FLT_EPSILON) {
+			ut_assert_true(fabsf((smooth.getMaxAcceleration() - acc_max_up) < FLT_EPSILON));
 
 		} else {
-			ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_down < FLT_EPSILON));
+			ut_assert_true(fabsf((smooth.getMaxAcceleration() - acc_max_down) < FLT_EPSILON));
 		}
 
 
@@ -124,9 +123,9 @@ bool SmoothZTest::brakeDownward()
 		 * We also set vel to previous setpoint where we make the assumption that
 		 * the vehicle can perfectly track the setpoints.
 		 */
-		vel_sp[1] = vel_sp[0];
-		vel_sp[0] = 0.0f;
-		vel = vel_sp[1];
+		vel_sp_previous = vel_sp_current;
+		vel_sp_current = 0.0f;
+		vel = vel_sp_previous;
 
 	}
 
@@ -137,13 +136,13 @@ bool SmoothZTest::accelerateUpwardFromBrake()
 {
 	/* Downward flight and want to stop */
 	float stick_current = -1.0f; // sticks are at full upward position
-	float vel_sp_current = -5.0f; // desired velocity is at -5m/s
+	float vel_sp_target = -5.0f; // desired velocity is at -5m/s
+	float vel_sp_current = vel_sp_target;
 	float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
 	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
 	float acc_max_up = 5.0f;
 	float acc_max_down = 2.0f;
 
-	float vel_sp[] = {vel_sp_current, vel_sp_previous};
 	ManualSmoothingZ smooth(vel, stick_current);
 
 	/* overwrite parameters since they might change depending on configuration */
@@ -155,27 +154,27 @@ bool SmoothZTest::accelerateUpwardFromBrake()
 
 	for (int i = 0; i < 100; i++) {
 
-		smooth.smoothVelFromSticks(vel_sp, dt);
+		smooth.smoothVelFromSticks(vel_sp_current, dt);
 
 		/* Test if intention is acceleration */
 		ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
 
 		/* we should always use upward acceleration */
-		ut_assert_true((smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON));
+		ut_assert_true((smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON));
 
 		/* New setpoint has to be larger than previous setpoint or equal to target velocity
 		 * vel_sp_current. The negative sign is because of NED frame.
 		 */
-		ut_assert_true((-vel_sp[0] > -vel_sp[1]) || (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON));
+		ut_assert_true((-vel_sp_current > -vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_previous) < FLT_EPSILON));
 
 
 		/* We reset the previous setpoint to newest setpoint and reset the current setpoint.
 		 * We also set the current velocity to the previous setpoint with the assumption that
 		 * the vehicle does perfect tracking.
 		 */
-		vel_sp[1] = vel_sp[0];
-		vel_sp[0] = vel_sp_current;
-		vel = vel_sp[1];
+		vel_sp_previous = vel_sp_current;
+		vel_sp_current = vel_sp_target;
+		vel = vel_sp_previous;
 
 	}
 
@@ -186,13 +185,13 @@ bool SmoothZTest::accelerateDownwardFromBrake()
 {
 	/* Downward flight and want to stop */
 	float stick_current = 1.0f; // sticks are at full downward position
-	float vel_sp_current = 5.0f; // desired velocity is at 5m/s
+	float vel_sp_target =  5.0f; // desired velocity is at 5m/s
+	float vel_sp_current = vel_sp_target;
 	float vel_sp_previous = 0.0f; // the demanded previous setpoint was 0m/s downwards
 	float vel = vel_sp_previous; // assume that current velocity is equal to previous vel setpoint
 	float acc_max_up = 5.0f;
 	float acc_max_down = 2.0f;
 
-	float vel_sp[] = {vel_sp_current, vel_sp_previous};
 	ManualSmoothingZ smooth(vel, stick_current);
 
 	/* overwrite parameters since they might change depending on configuration */
@@ -204,32 +203,32 @@ bool SmoothZTest::accelerateDownwardFromBrake()
 
 	for (int i = 0; i < 100; i++) {
 
-		smooth.smoothVelFromSticks(vel_sp, dt);
+		smooth.smoothVelFromSticks(vel_sp_current, dt);
 
 		/* Test if intention is acceleration */
 		ut_assert_true(smooth.getIntention() == ManualIntentionZ::acceleration);
 
 		/* we should always use downward acceleration except when target velocity is reached */
-		if (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON) {
-			ut_assert_true(smooth.getMaxAcceleration(vel_sp) - acc_max_up < FLT_EPSILON);
+		if (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON) {
+			ut_assert_true(smooth.getMaxAcceleration() - acc_max_up < FLT_EPSILON);
 
 		} else {
-			ut_assert_true(smooth.getMaxAcceleration(vel_sp) - acc_max_down < FLT_EPSILON);
+			ut_assert_true(smooth.getMaxAcceleration() - acc_max_down < FLT_EPSILON);
 		}
 
 		/* New setpoint has to be larger than previous setpoint or equal to target velocity
 		 * vel_sp_current (NED frame).
 		 */
-		ut_assert_true((vel_sp[0] > vel_sp[1]) || (fabsf(vel_sp[0] - vel_sp_current) < FLT_EPSILON));
+		ut_assert_true((vel_sp_current > vel_sp_previous) || (fabsf(vel_sp_current - vel_sp_target) < FLT_EPSILON));
 
 
 		/* We reset the previous setpoint to newest setpoint and reset the current setpoint.
 		 * We also set the current velocity to the previous setpoint with the assumption that
 		 * the vehicle does perfect tracking.
 		 */
-		vel_sp[1] = vel_sp[0];
-		vel_sp[0] = vel_sp_current;
-		vel = vel_sp[1];
+		vel_sp_previous = vel_sp_current;
+		vel_sp_current = vel_sp_target;
+		vel = vel_sp_previous;
 
 	}