flight tasks smoothing: refactor param_find -> Param

Plus some style fixes

src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp

@@ -39,8 +39,8 @@
 
 using namespace matrix;
 
-	_smoothing(_velocity(2), _sticks(2))
 FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth() :
+	_smoothing(this, _velocity(2), _sticks(2))
 {}
 
 void FlightTaskManualAltitudeSmooth::_updateSetpoints()

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp

@@ -40,8 +40,8 @@
 using namespace matrix;
 
 FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth() :
-	_smoothingXY(matrix::Vector2f(&_velocity(0))),
+	_smoothingXY(this, matrix::Vector2f(&_velocity(0))),
-	_smoothingZ(_velocity(2), _sticks(2))
+	_smoothingZ(this, _velocity(2), _sticks(2))
 {}
 
 void FlightTaskManualPositionSmooth::_updateSetpoints()

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

@@ -40,26 +40,16 @@
 #include <mathlib/mathlib.h>
 #include <float.h>
 
-ManualSmoothingXY::ManualSmoothingXY(const matrix::Vector2f &vel) :
+ManualSmoothingXY::ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel) :
+	ModuleParams(parent),
 	_vel_sp_prev(vel)
 {
-	_acc_hover_h = param_find("MPC_ACC_HOR_MAX");
-	_acc_xy_max_h = param_find("MPC_ACC_HOR");
-	_dec_xy_min_h = param_find("DEC_HOR_SLOW");
-	_jerk_max_h = param_find("MPC_JERK_MAX");
-	_jerk_min_h = param_find("MPC_JERK_MIN");
-	_vel_manual_h = param_find("MPC_VEL_MANUAL");
-
-	/* Load the params the very first time */
-	_setParams();
 }
 
 void
 ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
 				  const float &yawrate_sp, const float dt)
 {
-	_updateParams();
-
 	_updateAcceleration(vel_sp, vel, yaw, yawrate_sp, dt);
 
 	_velocitySlewRate(vel_sp, dt);
@@ -67,30 +57,6 @@ ManualSmoothingXY::smoothVelocity(matrix::Vector2f &vel_sp, const matrix::Vector
 	_vel_sp_prev = vel_sp;
 }
 
-void
-ManualSmoothingXY::_updateParams()
-{
-	bool updated;
-	parameter_update_s param_update;
-	orb_check(_parameter_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(parameter_update), _parameter_sub, &param_update);
-		_setParams();
-	}
-}
-
-void
-ManualSmoothingXY::_setParams()
-{
-	param_get(_acc_hover_h, &_acc_hover);
-	param_get(_acc_xy_max_h, &_acc_xy_max);
-	param_get(_dec_xy_min_h, &_dec_xy_min);
-	param_get(_jerk_max_h, &_jerk_max);
-	param_get(_jerk_min_h, &_jerk_min);
-	param_get(_vel_manual_h, &_vel_manual);
-}
-
 void
 ManualSmoothingXY::_updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel,  const float &yaw,
 				       const float &yawrate_sp, const float dt)
@@ -140,7 +106,7 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
 		 * Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s.
 		 * Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand. */
 		bool yawspeed_demand =  fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
-		bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual) && !yawspeed_demand
+		bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual.get()) && !yawspeed_demand
 					&& (vel.length() > 2.0f);
 		bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
 
@@ -176,7 +142,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
 			} else if (intention != _intention) {
 				/* we start braking with lowest acceleration
 				 * This make stopping smoother. */
-				_acc_state_dependent = _dec_xy_min;
+				_acc_state_dependent = _dec_xy_min.get();
 
 				/* Adjust jerk based on current velocity, This ensures
 				 * that the vehicle will stop much quicker at large speed but
@@ -184,10 +150,10 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
 				 */
 				_jerk_state_dependent = 1e6f; // default
 
-				if (_jerk_max > _jerk_min) {
+				if (_jerk_max.get() > _jerk_min.get()) {
 
-					_jerk_state_dependent = math::min((_jerk_max - _jerk_min)
+					_jerk_state_dependent = math::min((_jerk_max.get() - _jerk_min.get())
-									  / _vel_manual * vel.length() + _jerk_min, _jerk_max);
+									  / _vel_manual.get() * vel.length() + _jerk_min.get(), _jerk_max.get());
 				}
 
 				/* Since user wants to brake smoothly but NOT continuing to fly
@@ -197,14 +163,14 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
 			}
 
 			/* limit jerk when braking to zero */
-			float jerk = (_acc_hover - _acc_state_dependent) / dt;
+			float jerk = (_acc_hover.get() - _acc_state_dependent) / dt;
 
 			if (jerk > _jerk_state_dependent) {
 				_acc_state_dependent = _jerk_state_dependent * dt
 						       + _acc_state_dependent;
 
 			} else {
-				_acc_state_dependent = _acc_hover;
+				_acc_state_dependent = _acc_hover.get();
 			}
 
 			break;
@@ -221,20 +187,20 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
 			 * slewrate will have no effect. Nonetheless, just set
 			 * acceleration to maximum.
 			 */
-			_acc_state_dependent = _acc_xy_max;
+			_acc_state_dependent = _acc_xy_max.get();
 
 			break;
 		}
 
 	case Intention::acceleration: {
 			/* Limit acceleration linearly based on velocity setpoint.*/
-			_acc_state_dependent = (_acc_xy_max - _dec_xy_min)
+			_acc_state_dependent = (_acc_xy_max.get() - _dec_xy_min.get())
-					       / _vel_manual * vel_sp.length() + _dec_xy_min;
+					       / _vel_manual.get() * vel_sp.length() + _dec_xy_min.get();
 			break;
 		}
 
 	case Intention::deceleration: {
-			_acc_state_dependent = _dec_xy_min;
+			_acc_state_dependent = _dec_xy_min.get();
 			break;
 		}
 	}

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

@@ -39,16 +39,17 @@
 
 #pragma once
 
-#include <systemlib/param/param.h>
+#include <px4_module_params.h>
 #include <matrix/matrix/math.hpp>
 
-class ManualSmoothingXY
+class ManualSmoothingXY : public ModuleParams
 {
 public:
-	ManualSmoothingXY(const matrix::Vector2f &vel);
+	ManualSmoothingXY(ModuleParams *parent, const matrix::Vector2f &vel);
-	~ManualSmoothingXY() {};
+	~ManualSmoothingXY() = default;
 
-	/* Smoothing of velocity setpoint horizontally based
+	/**
+	 * Smoothing of velocity setpoint horizontally based
 	 * on flight direction.
 	 * @param vel_sp: velocity setpoint in xy
 	 * @param dt: time delta in seconds
@@ -70,13 +71,22 @@ public:
 	/* Overwrite methods:
 	 * Needed if different parameter values than default required.
 	 */
-	void overwriteHoverAcceleration(float acc) {_acc_hover = acc;};
+	void overwriteHoverAcceleration(float acc) { _acc_hover.set(acc); }
-	void overwriteMaxAcceleration(float acc) {_acc_xy_max = acc;};
+	void overwriteMaxAcceleration(float acc) { _acc_xy_max.set(acc); }
-	void overwriteDecelerationMin(float dec) {_dec_xy_min = dec;};
+	void overwriteDecelerationMin(float dec) { _dec_xy_min.set(dec); }
-	void overwriteJerkMax(float jerk) {_jerk_max = jerk;};
+	void overwriteJerkMax(float jerk) { _jerk_max.set(jerk); }
-	void overwriteJerkMin(float jerk) {_jerk_min = jerk;};
+	void overwriteJerkMin(float jerk) { _jerk_min.set(jerk); }
 
 private:
+	void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+				 const float &yawrate_sp, const float dt);
+	Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
+				const float &yawrate_sp);
+	void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention,
+				   const float dt);
+	void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt);
+	matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw);
+	matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw);
 
 	/* User intention: brake or acceleration */
 	Intention _intention{Intention::acceleration};
@@ -90,32 +100,13 @@ private:
 	/* Previous setpoints */
 	matrix::Vector2f _vel_sp_prev{}; // previous velocity setpoint
 
-	/* Params */
+	DEFINE_PARAMETERS(
-	param_t _acc_hover_h{PARAM_INVALID};
+		(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _acc_hover, ///< acceleration in hover
-	param_t _acc_xy_max_h{PARAM_INVALID};
+		(ParamFloat<px4::params::MPC_ACC_HOR>) _acc_xy_max, ///< acceleration in flight
-	param_t _dec_xy_min_h{PARAM_INVALID};
+		(ParamFloat<px4::params::MPC_DEC_HOR_SLOW>) _dec_xy_min, ///< deceleration in flight
-	param_t _jerk_max_h{PARAM_INVALID};
+		(ParamFloat<px4::params::MPC_JERK_MIN>) _jerk_min, ///< jerk min during brake
-	param_t _jerk_min_h{PARAM_INVALID};
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max, ///< jerk max during brake
-	param_t _vel_manual_h{PARAM_INVALID};
+		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _vel_manual ///< maximum velocity in manual controlled mode
-	float _acc_hover{50.0f}; // acceleration in hover
+	)
-	float _acc_xy_max{10.0f}; // acceleration in flight
-	float _dec_xy_min{1.0f}; // deceleration in flight
-	float _jerk_max{15.0f}; // jerk max during brake
-	float _jerk_min{1.0f}; // jerk min during brake
-	float _vel_manual{}; //maximum velocity in manual controlled mode
-	int _parameter_sub{-1};
-
-	/* Helper methods */
-	void _setParams();
-	void _updateParams();
-	void _updateAcceleration(matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
-				 const float &yawrate_sp, const float dt);
-	Intention _getIntention(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const float &yaw,
-				const float &yawrate_sp);
-	void _getStateAcceleration(const matrix::Vector2f &vel_sp, const matrix::Vector2f &vel, const Intention &intention,
-				   const float dt);
-	void _velocitySlewRate(matrix::Vector2f &vel_sp, const float dt);
-	matrix::Vector2f _getWorldToHeadingFrame(const matrix::Vector2f &vec, const float &yaw) ;
-	matrix::Vector2f _getHeadingToWorldFrame(const matrix::Vector2f &vec, const float &yaw);
 
 };

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

@@ -40,15 +40,10 @@
 #include <mathlib/mathlib.h>
 #include <float.h>
 
-ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) :
+ManualSmoothingZ::ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick) :
+	ModuleParams(parent),
 	_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");
-	_jerk_max_h = param_find("MPC_JERK_MAX");
-
-	/* Load the params the very first time */
-	setParams();
 }
 
 /* in manual altitude control apply acceleration limit based on stick input
@@ -59,33 +54,11 @@ ManualSmoothingZ::ManualSmoothingZ(const float &vel, const float &stick) :
 void
 ManualSmoothingZ::smoothVelFromSticks(float &vel_sp, const float dt)
 {
-	updateParams();
 	updateAcceleration(vel_sp, dt);
 	velocitySlewRate(vel_sp, dt);
 	_vel_sp_prev = vel_sp;
 }
 
-void
-ManualSmoothingZ::updateParams()
-{
-	bool updated;
-	parameter_update_s param_update;
-	orb_check(_parameter_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(parameter_update), _parameter_sub, &param_update);
-		setParams();
-	}
-}
-
-void
-ManualSmoothingZ::setParams()
-{
-	param_get(_acc_max_up_h, &_acc_max_up);
-	param_get(_acc_max_down_h, &_acc_max_down);
-	param_get(_jerk_max_h, &_jerk_max);
-}
-
 void
 ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
 {
@@ -109,7 +82,7 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
 	if ((_intention != ManualIntentionZ::brake) && (intention == ManualIntentionZ::brake)) {
 
 		/* we start with lowest acceleration */
-		_acc_state_dependent = _acc_max_down;
+		_acc_state_dependent = _acc_max_down.get();
 
 		/* reset slew-rate: this ensures that there
 		 * is no delay present when user demands to brake
@@ -123,13 +96,13 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
 	case ManualIntentionZ::brake: {
 
 			/* limit jerk when braking to zero */
-			float jerk = (_acc_max_up - _acc_state_dependent) / dt;
+			float jerk = (_acc_max_up.get() - _acc_state_dependent) / dt;
 
-			if (jerk > _jerk_max) {
+			if (jerk > _jerk_max.get()) {
-				_acc_state_dependent = _jerk_max * dt + _acc_state_dependent;
+				_acc_state_dependent = _jerk_max.get() * dt + _acc_state_dependent;
 
 			} else {
-				_acc_state_dependent = _acc_max_up;
+				_acc_state_dependent = _acc_max_up.get();
 			}
 
 			break;
@@ -137,8 +110,8 @@ ManualSmoothingZ::updateAcceleration(float &vel_sp, const float dt)
 
 	case ManualIntentionZ::acceleration: {
 
-			_acc_state_dependent = (getMaxAcceleration() - _acc_max_down)
+			_acc_state_dependent = (getMaxAcceleration() - _acc_max_down.get())
-					       * fabsf(_stick) + _acc_max_down;
+					       * fabsf(_stick) + _acc_max_down.get();
 			break;
 		}
 	}
@@ -153,11 +126,11 @@ ManualSmoothingZ::setMaxAcceleration()
 
 	if (_stick < -FLT_EPSILON) {
 		/* accelerating upward */
-		_max_acceleration =  _acc_max_up;
+		_max_acceleration =  _acc_max_up.get();
 
 	} else if (_stick > FLT_EPSILON) {
 		/* accelerating downward */
-		_max_acceleration = _acc_max_down;
+		_max_acceleration = _acc_max_down.get();
 
 	} else {
 
@@ -165,15 +138,15 @@ ManualSmoothingZ::setMaxAcceleration()
 
 		if (fabsf(_vel_sp_prev) < FLT_EPSILON) {
 			/* at rest */
-			_max_acceleration = _acc_max_up;
+			_max_acceleration = _acc_max_up.get();
 
 		} else if (_vel_sp_prev < 0.0f) {
 			/* braking downward */
-			_max_acceleration = _acc_max_down;
+			_max_acceleration = _acc_max_down.get();
 
 		} else {
 			/* braking upward */
-			_max_acceleration = _acc_max_up;
+			_max_acceleration = _acc_max_up.get();
 		}
 	}
 }

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

@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <systemlib/param/param.h>
+#include <px4_module_params.h>
 
 /* User intention: brake or acceleration */
 enum class ManualIntentionZ {
@@ -47,13 +47,14 @@ enum class ManualIntentionZ {
 	acceleration,
 };
 
-class ManualSmoothingZ
+class ManualSmoothingZ : public ModuleParams
 {
 public:
-	ManualSmoothingZ(const float &vel, const float &stick);
+	ManualSmoothingZ(ModuleParams *parent, const float &vel, const float &stick);
-	~ManualSmoothingZ() {};
+	~ManualSmoothingZ() = default;
 
-	/* Smooths velocity setpoint based
+	/**
+	 * 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
@@ -70,11 +71,14 @@ public:
 	/* Overwrite methods:
 	 * Needed if different parameter values than default required.
 	 */
-	void overwriteAccelerationUp(float acc_max_up) {_acc_max_up = acc_max_up;};
+	void overwriteAccelerationUp(float acc_max_up) { _acc_max_up.set(acc_max_up); }
-	void overwriteAccelerationDown(float acc_max_down) {_acc_max_down = acc_max_down;};
+	void overwriteAccelerationDown(float acc_max_down)  {_acc_max_down.set(acc_max_down); }
-	void overwriteJerkMax(float jerk_max) {_jerk_max = jerk_max;};
+	void overwriteJerkMax(float jerk_max)  {_jerk_max.set(jerk_max); }
 
 private:
+	void velocitySlewRate(float &vel_sp, const float dt);
+	void updateAcceleration(float &vel_sp, const float dt);
+	void setMaxAcceleration();
 
 	/* User intention: brake or acceleration */
 	ManualIntentionZ _intention{ManualIntentionZ::acceleration};
@@ -90,22 +94,11 @@ private:
 	 */
 	float _acc_state_dependent{0.0f};
 	float _vel_sp_prev;
-	float _max_acceleration;
+	float _max_acceleration{0.f};
-
-	/* 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};
-	int _parameter_sub{-1};
-
-	/* Helper methods */
-	void velocitySlewRate(float &vel_sp, const float dt);
-	void setParams();
-	void updateParams();
-	void updateAcceleration(float &vel_sp, const float dt);
-	void setMaxAcceleration();
 
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _acc_max_up,
+		(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _acc_max_down,
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _jerk_max
+	)
 };

src/systemcmds/tests/test_smooth_z.cpp

@@ -34,7 +34,7 @@ bool SmoothZTest::brakeUpward()
 	float acc_max_up = 5.0f;
 	float acc_max_down = 2.0f;
 
-	ManualSmoothingZ smooth(vel, stick_current);
+	ManualSmoothingZ smooth(nullptr, vel, stick_current);
 
 	/* overwrite parameters since they might change depending on configuration */
 	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
@@ -86,7 +86,7 @@ bool SmoothZTest::brakeDownward()
 	float acc_max_up = 5.0f;
 	float acc_max_down = 2.0f;
 
-	ManualSmoothingZ smooth(vel, stick_current);
+	ManualSmoothingZ smooth(nullptr, vel, stick_current);
 
 	/* overwrite parameters since they might change depending on configuration */
 	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
@@ -143,7 +143,7 @@ bool SmoothZTest::accelerateUpwardFromBrake()
 	float acc_max_up = 5.0f;
 	float acc_max_down = 2.0f;
 
-	ManualSmoothingZ smooth(vel, stick_current);
+	ManualSmoothingZ smooth(nullptr, vel, stick_current);
 
 	/* overwrite parameters since they might change depending on configuration */
 	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss
@@ -192,7 +192,7 @@ bool SmoothZTest::accelerateDownwardFromBrake()
 	float acc_max_up = 5.0f;
 	float acc_max_down = 2.0f;
 
-	ManualSmoothingZ smooth(vel, stick_current);
+	ManualSmoothingZ smooth(nullptr, vel, stick_current);
 
 	/* overwrite parameters since they might change depending on configuration */
 	smooth.overwriteAccelerationDown(acc_max_down); // downward max acceleration of 2m/ss