ManualSmoothingZ: comments cleanup and style fix

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

@@ -34,7 +34,7 @@
 /**
  * @file ManualSmoothingZ.hpp
  *
- * This Class is used for smoothing the velocity setpoints in Z-direction.
+ * This Class is used for smoothing the velocity set-points in z-direction NED frame.
  */
 
 #include "ManualSmoothingZ.hpp"
@@ -47,7 +47,7 @@ ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) :
 {
 	_acc_max_up_h = param_find("MPC_ACC_UP_MAX");
 	_acc_max_down_h = param_find("MPC_ACC_DOWN_MAX");
-	_jerk_max = param_find("MPC_JERK_MAX");
+	_jerk_max_h = param_find("MPC_JERK_MAX");
 
 	/* Load the params the very first time */
 	setParams();
@@ -98,23 +98,23 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
 	const bool is_current_zero = (fabsf(_stick) <= FLT_EPSILON);
 
 	/* default is acceleration */
-	Intention intention = Intention::acceleration;
+	ManualIntentionZ intention = ManualIntentionZ::acceleration;
 
 	/* check zero input stick */
 	if (is_current_zero) {
-		intention = Intention::brake;
+		intention = ManualIntentionZ::brake;
 	}
 
 	/*
 	 * update intention
 	 */
-	if ((_intention != Intention::brake) && (intention == Intention::brake)) {
+	if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
 
 		/* we start with lowest acceleration */
 		_acc_state_dependent = _acc_max_down;
 
-		/* reset slewrate: this ensures that there
-		 * is no delay present because of the slewrate
+		/* reset slew-rate: this ensures that there
+		 * is no delay present when user demands to brake
 		 */
 
 		vel_sp[1] = _vel;
@@ -122,7 +122,7 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
 	}
 
 	switch (intention) {
-	case Intention::brake: {
+	case ManualIntentionZ::brake: {
 
 			/* limit jerk when braking to zero */
 			float jerk = (_acc_max_up - _acc_state_dependent) / dt;
@@ -137,10 +137,9 @@ ManualSmoothingZ::updateAcceleration(float vel_sp[2], const float dt)
 			break;
 		}
 
-	case Intention::acceleration: {
+	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;
 		}
@@ -170,7 +169,7 @@ ManualSmoothingZ::getMaxAcceleration(float vel_sp[2])
 			/* at rest */
 			return _acc_max_up;
 
-		} else if (vel_sp[0] < 0.0f ) {
+		} else if (vel_sp[0] < 0.0f) {
 			/* braking downward */
 			return _acc_max_down;
 

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

@@ -41,39 +41,68 @@
 
 #include <systemlib/param/param.h>
 
+/* User intention: brake or acceleration */
+enum class ManualIntentionZ {
+	brake,
+	acceleration,
+};
 
 class ManualSmoothingZ
 {
 public:
 	ManualSmoothingZ(const float &vel, const float &stick);
-	~ManualSmoothingZ();
+	~ManualSmoothingZ() {};
 
+	/* Smooths velocity setpoint based
+	 * on flight direction.
+	 * @param vel_sp[2] array: vel_sp[0] = current velocity set-point;
+	 * 					 	   vel_sp[1] = previous velocity set-point
+	 * 		  vel_sp will contain smoothed current previous set-point.
+	 * @param dt: time delta in seconds
+	 */
 	void smoothVelFromSticks(float vel_sp[2], const float dt);
 
+
+	/* Getter methods */
+	float getMaxAcceleration(float vel_sp[2]);
+	ManualIntentionZ getIntention() {return _intention;};
+
+	/* Overwrite methods:
+	 * Needed if different parameter values than default required.
+	 */
+	void overwriteAccelerationUp(float acc_max_up) {_acc_max_up = acc_max_up;};
+	void overwriteAccelerationDown(float acc_max_down) {_acc_max_down = acc_max_down;};
+	void overwriteJerkMax(float jerk_max) {_jerk_max = jerk_max;};
+
 private:
 
-	enum class Intention {
-		brake,
-		acceleration,
-	};
-	Intention _intention{Intention::brake};
+	/* User intention: brake or acceleration */
+	ManualIntentionZ _intention{ManualIntentionZ::acceleration};
 
+	/* Dependency injection: vehicle velocity in z-direction;
+	 * stick input in z-direction
+	 */
 	const float &_vel;
 	const float &_stick;
 
+	/* Acceleration that depends on vehicle state
+	 * _acc_max_down <= _acc_state_dependent <= _acc_max_up
+	 */
+	float _acc_state_dependent{0.0f};
+
+	/* Params */
 	param_t _acc_max_up_h{PARAM_INVALID};
 	param_t _acc_max_down_h{PARAM_INVALID};
 	param_t _jerk_max_h{PARAM_INVALID};
 	float _acc_max_up{0.0f};
 	float _acc_max_down{0.0f};
 	float _jerk_max{10000.0f};
-	float _acc_state_dependent{0.0f};
 	int _parameter_sub{-1};
 
-
+	/* Helper methods */
 	void velocitySlewRate(float vel_sp[2], const float dt);
-	void updateParams();
-	void updateAcceleration(float &vel_sp_prev, const float dt);
 	void setParams();
-	float getMaxAcceleration();
+	void updateParams();
+	void updateAcceleration(float vel_sp[2], const float dt);
+
 };