From 1cc6732ec7b75c7c66fa9ad61b6688c9e8d46c37 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 2 Sep 2022 13:27:34 -0400 Subject: [PATCH] mc_pos_control: add velocity filter and remove controllib usage --- .../controllib/controllib_test/CMakeLists.txt | 1 + src/lib/mathlib/math/Functions.hpp | 5 + src/lib/mathlib/math/filter/AlphaFilter.hpp | 149 +++++++++++++++--- src/lib/mathlib/math/test/AlphaFilterTest.cpp | 6 +- src/modules/mc_pos_control/CMakeLists.txt | 1 - .../MulticopterPositionControl.cpp | 124 ++++++--------- .../MulticopterPositionControl.hpp | 18 +-- .../PositionControl/PositionControl.hpp | 8 +- .../mc_pos_control/mc_pos_control_params.c | 17 +- 9 files changed, 210 insertions(+), 119 deletions(-) diff --git a/src/lib/controllib/controllib_test/CMakeLists.txt b/src/lib/controllib/controllib_test/CMakeLists.txt index a6fed39a21..dc1f0dc443 100644 --- a/src/lib/controllib/controllib_test/CMakeLists.txt +++ b/src/lib/controllib/controllib_test/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( SRCS controllib_test_main.cpp DEPENDS + controllib ) diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index b81459ff67..41025563aa 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/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(); diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 0635ae438c..545e7f5de5 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -48,7 +48,7 @@ using namespace math; template -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}; }; diff --git a/src/lib/mathlib/math/test/AlphaFilterTest.cpp b/src/lib/mathlib/math/test/AlphaFilterTest.cpp index 30661ea82b..0f520c0853 100644 --- a/src/lib/mathlib/math/test/AlphaFilterTest.cpp +++ b/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)); diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 49f61ac967..7b814f8b3f 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -44,7 +44,6 @@ px4_add_module( DEPENDS PositionControl Takeoff - controllib geo SlewRate ) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 039bef28fd..a5e3ac0411 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #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) { diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 7a6be18ccd..0d725ed89c 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -41,7 +41,7 @@ #include "Takeoff/Takeoff.hpp" #include -#include +#include #include #include #include @@ -67,8 +67,8 @@ using namespace time_literals; -class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, - public ModuleParams, public px4::ScheduledWorkItem +class MulticopterPositionControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem { public: MulticopterPositionControl(bool vtol = false); @@ -145,6 +145,8 @@ private: (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, (ParamBool) _param_mpc_use_hte, + (ParamFloat) _param_mpc_vel_lp, + (ParamFloat) _param_mpc_veld_lp, // Takeoff / Land (ParamFloat) _param_com_spoolup_time, /**< time to let motors spool up after arming */ @@ -178,9 +180,8 @@ private: (ParamFloat) _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 _vel_filter{}; + AlphaFilter _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. diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 124fd08b03..d23a3c031e 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -46,10 +46,10 @@ #include 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}; }; /** diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index df0fb22d45..4cf4c2f907 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/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);