mc_pos_control: add velocity filter and remove controllib usage

src/lib/controllib/controllib_test/CMakeLists.txt

@@ -38,6 +38,7 @@ px4_add_module(
 	SRCS
 		controllib_test_main.cpp
 	DEPENDS
+		controllib
 	)
 
 

src/lib/mathlib/math/Functions.hpp

@@ -290,6 +290,11 @@ inline bool isFinite(const float &value)
 	return PX4_ISFINITE(value);
 }
 
+inline bool isFinite(const matrix::Vector2f &value)
+{
+	return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1));
+}
+
 inline bool isFinite(const matrix::Vector3f &value)
 {
 	return value.isAllFinite();

src/lib/mathlib/math/filter/AlphaFilter.hpp

@@ -48,7 +48,7 @@
 using namespace math;
 
 template <typename T>
-class AlphaFilter
+class AlphaFilter final
 {
 public:
 	AlphaFilter() = default;
@@ -56,6 +56,13 @@ public:
 
 	~AlphaFilter() = default;
 
+	// 0.01 to 10,000 Hz
+	static constexpr float MIN_FREQ_HZ = 0.01f;    // 0.01 Hz or 100s interval
+	static constexpr float MAX_FREQ_HZ = 10'000.f; // 10,000 Hz or 0.0001s interval
+
+	static constexpr float min_interval_s() { return 1.f / MAX_FREQ_HZ; }
+	static constexpr float max_interval_s() { return 1.f / MIN_FREQ_HZ; }
+
 	/**
 	 * Set filter parameters for time abstraction
 	 *
@@ -64,27 +71,67 @@ public:
 	 * @param sample_interval interval between two samples
 	 * @param time_constant filter time constant determining convergence
 	 */
-	void setParameters(float sample_interval, float time_constant)
+	bool setParameters(const float sample_interval, const float time_constant)
 	{
-		const float denominator = time_constant + sample_interval;
-
-		if (denominator > FLT_EPSILON) {
-			setAlpha(sample_interval / denominator);
-		}
-	}
-
-	bool setCutoffFreq(float sample_freq, float cutoff_freq)
-	{
-		if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) || (cutoff_freq >= sample_freq / 2.f)
-		    || !isFinite(sample_freq) || !isFinite(cutoff_freq)) {
-
-			// Invalid parameters
+		if ((sample_interval <= 0.f) || !PX4_ISFINITE(sample_interval)
+		    || (time_constant <= 0.f) || !PX4_ISFINITE(time_constant)) {
+			// invalid parameters, disable filter
+			disable();
 			return false;
 		}
 
-		setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq));
-		_cutoff_freq = cutoff_freq;
-		return true;
+		_sample_interval = math::constrain(sample_interval, min_interval_s(), max_interval_s());
+		_time_constant = time_constant;
+
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
+	}
+
+	bool setCutoffFreq(const float sample_freq, const float cutoff_freq)
+	{
+		if ((sample_freq <= 0.f) || !PX4_ISFINITE(sample_freq)
+		    || (cutoff_freq <= 0.f) || !PX4_ISFINITE(cutoff_freq)) {
+			// invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		const float sample_freq_constrained = math::constrain(sample_freq, MIN_FREQ_HZ, MAX_FREQ_HZ);
+		const float cutoff_freq_constrained = math::constrain(cutoff_freq, MIN_FREQ_HZ, sample_freq_constrained / 2.f);
+
+		_sample_interval = 1.f / sample_freq_constrained;
+		_time_constant = 1.f / (2.f * M_PI_F * cutoff_freq_constrained);
+
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
+	}
+
+	bool setCutoffFrequency(const float cutoff_freq)
+	{
+		const float time_constant = 1.f / (2.f * M_PI_F * cutoff_freq);
+		return setTimeConstant(time_constant);
+	}
+
+	bool setTimeConstant(const float time_constant)
+	{
+		if ((time_constant <= 0.f) || !PX4_ISFINITE(time_constant)) {
+			// Invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		if ((fabsf(time_constant - _time_constant) / _time_constant) < 0.01f) {
+			// no change, do nothing
+			return true;
+		}
+
+		// time constant changed, update alpha
+		_time_constant = time_constant;
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
 	}
 
 	/**
@@ -92,14 +139,35 @@ public:
 	 *
 	 * @param alpha [0,1] filter weight for the previous state. High value - long time constant.
 	 */
-	void setAlpha(float alpha) { _alpha = alpha; }
+	bool setAlpha(const float alpha)
+	{
+		if (alpha < 0.f || alpha > 1.f || !PX4_ISFINITE(alpha)) {
+			// Invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		_alpha = alpha;
+		return true;
+	}
 
 	/**
 	 * Set filter state to an initial value
 	 *
 	 * @param sample new initial value
 	 */
-	void reset(const T &sample) { _filter_state = sample; }
+	const T &reset(const T &sample = {})
+	{
+		_filter_state = sample;
+		return _filter_state;
+	}
+
+	void disable()
+	{
+		_alpha = 1.f;
+		_sample_interval = 1.f;
+		_time_constant = 0.f;
+	}
 
 	/**
 	 * Add a new raw value to the filter
@@ -112,13 +180,44 @@ public:
 		return _filter_state;
 	}
 
+	const T &update(const T &sample, const float dt)
+	{
+		if ((fabsf(dt - _sample_interval) / _sample_interval) > 0.01f) {
+			// update parameters if changed by more than 1%
+			if (!setParameters(dt, _time_constant)) {
+				// invalid new parameters, reset filter
+				return reset(sample);
+			}
+		}
+
+		_filter_state = updateCalculation(sample);
+		return _filter_state;
+	}
+
 	const T &getState() const { return _filter_state; }
-	float getCutoffFreq() const { return _cutoff_freq; }
 
-protected:
-	T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; }
+	float getCutoffFreq() const { return 1.f / (2.f * M_PI_F * _time_constant); }
+
+	const float &getSampleInterval() const { return _sample_interval; }
+	const float &getTimeConstant() const { return _time_constant; }
+
+private:
+
+	T updateCalculation(const T &sample)
+	{
+		// don't allow bad updates to propagate
+		const T ret = (1.f - _alpha) * _filter_state + _alpha * sample;
+
+		if (isFinite(ret)) {
+			return ret;
+		}
+
+		return {};
+	}
 
-	float _cutoff_freq{0.f};
-	float _alpha{0.f};
 	T _filter_state{};
+	float _alpha{1.f};
+
+	float _sample_interval{1.f};
+	float _time_constant{0.f};
 };

src/lib/mathlib/math/test/AlphaFilterTest.cpp

@@ -247,7 +247,11 @@ TEST(AlphaFilterTest, SetFrequencyTest)
 	const float fs = .1f;
 
 	EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, 0.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, fs)); // Cutoff above Nyquist freq
+
+	// Cutoff above Nyquist freq
+	EXPECT_TRUE(_alpha_filter.setCutoffFreq(fs, fs));
+	EXPECT_FLOAT_EQ(_alpha_filter.getCutoffFreq(), fs / 2.f);
+
 	EXPECT_FALSE(_alpha_filter.setCutoffFreq(0.f, fs / 4.f));
 	EXPECT_FALSE(_alpha_filter.setCutoffFreq(0.f, 0.f));
 	EXPECT_FALSE(_alpha_filter.setCutoffFreq(-fs, fs / 4.f));

src/modules/mc_pos_control/CMakeLists.txt

@@ -44,7 +44,6 @@ px4_add_module(
 	DEPENDS
 		PositionControl
 		Takeoff
-		controllib
 		geo
 		SlewRate
 	)

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -35,6 +35,7 @@
 
 #include <float.h>
 #include <lib/mathlib/mathlib.h>
+#include <lib/mathlib/math/Functions.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <px4_platform_common/events.h>
 #include "PositionControl/ControlMath.hpp"
@@ -42,13 +43,9 @@
 using namespace matrix;
 
 MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
-	SuperBlock(nullptr, "MPC"),
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
-	_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
-	_vel_x_deriv(this, "VELD"),
-	_vel_y_deriv(this, "VELD"),
-	_vel_z_deriv(this, "VELD")
+	_vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint))
 {
 	parameters_update(true);
 	_tilt_limit_slew_rate.setSlewRate(.2f);
@@ -83,7 +80,6 @@ void MulticopterPositionControl::parameters_update(bool force)
 
 		// update parameters from storage
 		ModuleParams::updateParams();
-		SuperBlock::updateParams();
 
 		int num_changed = 0;
 
@@ -257,67 +253,16 @@ void MulticopterPositionControl::parameters_update(bool force)
 		_param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
 		_param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));
 
+		// velocity and velocity derivative filters
+		_vel_filter.setCutoffFrequency(_param_mpc_vel_lp.get());
+		_vel_derivative_filter.setCutoffFrequency(_param_mpc_veld_lp.get());
+
 		_takeoff.setSpoolupTime(_param_com_spoolup_time.get());
 		_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
 		_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
 	}
 }
 
-PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s
-		&vehicle_local_position)
-{
-	PositionControlStates states;
-
-	const Vector2f position_xy(vehicle_local_position.x, vehicle_local_position.y);
-
-	// only set position states if valid and finite
-	if (vehicle_local_position.xy_valid && position_xy.isAllFinite()) {
-		states.position.xy() = position_xy;
-
-	} else {
-		states.position(0) = states.position(1) = NAN;
-	}
-
-	if (PX4_ISFINITE(vehicle_local_position.z) && vehicle_local_position.z_valid) {
-		states.position(2) = vehicle_local_position.z;
-
-	} else {
-		states.position(2) = NAN;
-	}
-
-	const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy);
-
-	if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) {
-		states.velocity.xy() = velocity_xy;
-		states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0));
-		states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1));
-
-	} else {
-		states.velocity(0) = states.velocity(1) = NAN;
-		states.acceleration(0) = states.acceleration(1) = NAN;
-
-		// reset derivatives to prevent acceleration spikes when regaining velocity
-		_vel_x_deriv.reset();
-		_vel_y_deriv.reset();
-	}
-
-	if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) {
-		states.velocity(2) = vehicle_local_position.vz;
-		states.acceleration(2) = _vel_z_deriv.update(states.velocity(2));
-
-	} else {
-		states.velocity(2) = NAN;
-		states.acceleration(2) = NAN;
-
-		// reset derivative to prevent acceleration spikes when regaining velocity
-		_vel_z_deriv.reset();
-	}
-
-	states.yaw = vehicle_local_position.heading;
-
-	return states;
-}
-
 void MulticopterPositionControl::Run()
 {
 	if (should_exit()) {
@@ -337,10 +282,13 @@ void MulticopterPositionControl::Run()
 	if (_local_pos_sub.update(&vehicle_local_position)) {
 		const float dt =
 			math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
-		_time_stamp_last_loop = vehicle_local_position.timestamp_sample;
 
-		// set _dt in controllib Block for BlockDerivative
-		setDt(dt);
+		if (hrt_elapsed_time(&_time_stamp_last_loop) > 1_s) {
+			_vel_filter.reset();
+			_vel_derivative_filter.reset();
+		}
+
+		_time_stamp_last_loop = vehicle_local_position.timestamp_sample;
 
 		if (_vehicle_control_mode_sub.updated()) {
 			const bool previous_position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled;
@@ -375,10 +323,20 @@ void MulticopterPositionControl::Run()
 			if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
 				_setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
 				_setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
+
+				// adjust velocity filter state
+				_vel_filter.reset(Vector3f(_vel_filter.getState()(0) + vehicle_local_position.delta_vxy[0],
+							   _vel_filter.getState()(1) + vehicle_local_position.delta_vxy[1],
+							   _vel_filter.getState()(2)));
 			}
 
 			if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
 				_setpoint.velocity[2] += vehicle_local_position.delta_vz;
+
+				// adjust velocity filter state
+				_vel_filter.reset(Vector3f(_vel_filter.getState()(0),
+							   _vel_filter.getState()(1),
+							   _vel_filter.getState()(2) + vehicle_local_position.delta_vz));
 			}
 
 			if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
@@ -395,15 +353,6 @@ void MulticopterPositionControl::Run()
 			}
 		}
 
-		if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
-			_vel_x_deriv.reset();
-			_vel_y_deriv.reset();
-		}
-
-		if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
-			_vel_z_deriv.reset();
-		}
-
 		// save latest reset counters
 		_vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
 		_vz_reset_counter = vehicle_local_position.vz_reset_counter;
@@ -412,7 +361,34 @@ void MulticopterPositionControl::Run()
 		_heading_reset_counter = vehicle_local_position.heading_reset_counter;
 
 
-		PositionControlStates states{set_vehicle_states(vehicle_local_position)};
+		PositionControlStates states{};
+
+		// position
+		if (vehicle_local_position.xy_valid && vehicle_local_position.z_valid) {
+			states.position = Vector3f(vehicle_local_position.x, vehicle_local_position.y, vehicle_local_position.z);
+
+		} else if (vehicle_local_position.z_valid) {
+			// only z valid
+			states.position(2) = vehicle_local_position.z;
+		}
+
+		// velocity & velocity derivative
+		const Vector3f vel_filtered_prev = _vel_filter.getState();
+		_vel_filter.update(Vector3f(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz), dt);
+		_vel_derivative_filter.update((_vel_filter.getState() - vel_filtered_prev) / dt, dt);
+
+		if (vehicle_local_position.v_xy_valid && vehicle_local_position.v_z_valid) {
+			states.velocity = _vel_filter.getState();
+			states.acceleration = _vel_derivative_filter.getState();
+
+		} else if (vehicle_local_position.z_valid) {
+			// only vz valid
+			states.velocity(2) = _vel_filter.getState()(2);
+			states.acceleration(2) = _vel_derivative_filter.getState()(2);
+		}
+
+		// yaw
+		states.yaw = vehicle_local_position.heading;
 
 
 		if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -41,7 +41,7 @@
 #include "Takeoff/Takeoff.hpp"
 
 #include <drivers/drv_hrt.h>
-#include <lib/controllib/blocks.hpp>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <lib/perf/perf_counter.h>
 #include <lib/slew_rate/SlewRateYaw.hpp>
 #include <lib/systemlib/mavlink_log.h>
@@ -67,8 +67,8 @@
 
 using namespace time_literals;
 
-class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public control::SuperBlock,
-	public ModuleParams, public px4::ScheduledWorkItem
+class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, public ModuleParams,
+	public px4::ScheduledWorkItem
 {
 public:
 	MulticopterPositionControl(bool vtol = false);
@@ -145,6 +145,8 @@ private:
 		(ParamFloat<px4::params::MPC_TILTMAX_AIR>)  _param_mpc_tiltmax_air,
 		(ParamFloat<px4::params::MPC_THR_HOVER>)    _param_mpc_thr_hover,
 		(ParamBool<px4::params::MPC_USE_HTE>)       _param_mpc_use_hte,
+		(ParamFloat<px4::params::MPC_VEL_LP>)       _param_mpc_vel_lp,
+		(ParamFloat<px4::params::MPC_VELD_LP>)      _param_mpc_veld_lp,
 
 		// Takeoff / Land
 		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time, /**< time to let motors spool up after arming */
@@ -178,9 +180,8 @@ private:
 		(ParamFloat<px4::params::MPC_Z_VEL_ALL>)    _param_mpc_z_vel_all
 	);
 
-	control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
-	control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */
-	control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */
+	AlphaFilter<matrix::Vector3f> _vel_filter{};
+	AlphaFilter<matrix::Vector3f> _vel_derivative_filter{};
 
 	PositionControl _control;  /**< class for core PID position control */
 
@@ -213,11 +214,6 @@ private:
 	 */
 	void parameters_update(bool force);
 
-	/**
-	 * Check for validity of positon/velocity states.
-	 */
-	PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos);
-
 	/**
 	 * Generate setpoint to bridge no executable setpoint being available.
 	 * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid.

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -46,10 +46,10 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 
 struct PositionControlStates {
-	matrix::Vector3f position;
-	matrix::Vector3f velocity;
-	matrix::Vector3f acceleration;
-	float yaw;
+	matrix::Vector3f position{NAN, NAN, NAN};
+	matrix::Vector3f velocity{NAN, NAN, NAN};
+	matrix::Vector3f acceleration{NAN, NAN, NAN};
+	float yaw{NAN};
 };
 
 /**

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -551,12 +551,23 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.8f);
 PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.6f);
 
 /**
- * Low pass filter cut freq. for numerical velocity derivative
+ * Velocity low pass filter cutoff frequency
  *
  * @unit Hz
- * @min 0.0
+ * @min 10
+ * @max 100
+ * @decimal 1
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_VEL_LP, 40.0f);
+
+/**
+ * Velocity derivative low pass filter cutoff frequency
+ *
+ * @unit Hz
+ * @min 1
  * @max 10
- * @decimal 2
+ * @decimal 1
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f);