From 8b9900cce3f8e365039ae9fe80072be8ab425ec5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 18 Jun 2024 11:47:19 -0400 Subject: [PATCH] mc_pos_control: new velocity low pass and notch filter (optional, disabled by default) - MPC_VEL_LP: new velocity first order low pass filter (off by default) - MPC_VEL_NF_FRQ/MPC_VEL_NF_BW: new velocity notch filter (off by default) - MPC_VELD_LP: existing velocity derivative low pass filter, but I've dropped the remaining controllib usage --- src/lib/mathlib/math/Functions.hpp | 5 + .../MulticopterPositionControl.cpp | 91 ++++++++++++++----- .../MulticopterPositionControl.hpp | 28 ++++-- .../multicopter_position_control_params.c | 49 +++++++++- 4 files changed, 141 insertions(+), 32 deletions(-) diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 7fb54442d9..77cdb33c34 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 value.isAllFinite(); +} + inline bool isFinite(const matrix::Vector3f &value) { return value.isAllFinite(); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 9092892183..5f8e5f680b 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -42,14 +42,11 @@ 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)) { + _sample_interval_s.update(0.01f); // 100 Hz default parameters_update(true); _tilt_limit_slew_rate.setSlewRate(.2f); _takeoff_status_pub.advertise(); @@ -83,7 +80,42 @@ void MulticopterPositionControl::parameters_update(bool force) // update parameters from storage ModuleParams::updateParams(); - SuperBlock::updateParams(); + + float sample_freq_hz = 1.f / _sample_interval_s.mean(); + + // velocity notch filter + if ((_param_mpc_vel_nf_frq.get() > 0.f) && (_param_mpc_vel_nf_bw.get() > 0.f)) { + _vel_xy_notch_filter.setParameters(sample_freq_hz, _param_mpc_vel_nf_frq.get(), _param_mpc_vel_nf_bw.get()); + _vel_z_notch_filter.setParameters(sample_freq_hz, _param_mpc_vel_nf_frq.get(), _param_mpc_vel_nf_bw.get()); + + } else { + _vel_xy_notch_filter.disable(); + _vel_z_notch_filter.disable(); + } + + // velocity xy/z low pass filter + if (_param_mpc_vel_lp.get() > 0.f) { + _vel_xy_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_vel_lp.get()); + _vel_z_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_vel_lp.get()); + + } else { + // disable filtering + _vel_xy_lp_filter.setAlpha(1.f); + _vel_z_lp_filter.setAlpha(1.f); + } + + // velocity derivative xy/z low pass filter + if (_param_mpc_veld_lp.get() > 0.f) { + _vel_deriv_xy_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_veld_lp.get()); + _vel_deriv_z_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_veld_lp.get()); + + } else { + // disable filtering + _vel_deriv_xy_lp_filter.setAlpha(1.f); + _vel_deriv_z_lp_filter.setAlpha(1.f); + } + + int num_changed = 0; @@ -276,7 +308,7 @@ void MulticopterPositionControl::parameters_update(bool force) } PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s - &vehicle_local_position) + &vehicle_local_position, const float dt_s) { PositionControlStates states; @@ -300,29 +332,42 @@ PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehic 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)); + const Vector2f vel_xy_prev = _vel_xy_lp_filter.getState(); + + // vel xy notch filter, then low pass filter + states.velocity.xy() = _vel_xy_lp_filter.update(_vel_xy_notch_filter.apply(velocity_xy)); + + // vel xy derivative low pass filter + states.acceleration.xy() = _vel_deriv_xy_lp_filter.update((_vel_xy_lp_filter.getState() - vel_xy_prev) / dt_s); } 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(); + // reset filters to prevent acceleration spikes when regaining velocity + _vel_xy_lp_filter.reset({}); + _vel_xy_notch_filter.reset(); + _vel_deriv_xy_lp_filter.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)); + + const float vel_z_prev = _vel_z_lp_filter.getState(); + + // vel z notch filter, then low pass filter + states.velocity(2) = _vel_z_lp_filter.update(_vel_z_notch_filter.apply(vehicle_local_position.vz)); + + // vel z derivative low pass filter + states.acceleration(2) = _vel_deriv_z_lp_filter.update((_vel_z_lp_filter.getState() - vel_z_prev) / dt_s); } else { states.velocity(2) = NAN; states.acceleration(2) = NAN; - // reset derivative to prevent acceleration spikes when regaining velocity - _vel_z_deriv.reset(); + // reset filters to prevent acceleration spikes when regaining velocity + _vel_z_lp_filter.reset({}); + _vel_z_notch_filter.reset(); + _vel_deriv_z_lp_filter.reset({}); } states.yaw = vehicle_local_position.heading; @@ -351,8 +396,7 @@ void MulticopterPositionControl::Run() 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); + _sample_interval_s.update(dt); if (_vehicle_control_mode_sub.updated()) { const bool previous_position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled; @@ -380,7 +424,7 @@ void MulticopterPositionControl::Run() } } - PositionControlStates states{set_vehicle_states(vehicle_local_position)}; + PositionControlStates states{set_vehicle_states(vehicle_local_position, dt)}; // if a goto setpoint available this publishes a trajectory setpoint to go there if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample, @@ -638,12 +682,13 @@ void MulticopterPositionControl::adjustSetpointForEKFResets(const vehicle_local_ } if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { - _vel_x_deriv.reset(); - _vel_y_deriv.reset(); + _vel_xy_lp_filter.reset(_vel_xy_lp_filter.getState() + Vector2f(vehicle_local_position.delta_vxy)); + _vel_xy_notch_filter.reset(); } if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { - _vel_z_deriv.reset(); + _vel_z_lp_filter.reset(_vel_z_lp_filter.getState() + vehicle_local_position.delta_vz); + _vel_z_notch_filter.reset(); } // save latest reset counters diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 3dac59bc1b..f7cd9bbbe0 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -42,7 +42,9 @@ #include "GotoControl/GotoControl.hpp" #include -#include +#include +#include +#include #include #include #include @@ -68,8 +70,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); @@ -148,6 +150,11 @@ private: (ParamBool) _param_mpc_use_hte, (ParamBool) _param_mpc_acc_decouple, + (ParamFloat) _param_mpc_vel_lp, + (ParamFloat) _param_mpc_vel_nf_frq, + (ParamFloat) _param_mpc_vel_nf_bw, + (ParamFloat) _param_mpc_veld_lp, + // Takeoff / Land (ParamFloat) _param_com_spoolup_time, /**< time to let motors spool up after arming */ (ParamBool) _param_com_throw_en, /**< throw launch enabled */ @@ -185,9 +192,16 @@ private: (ParamFloat) _param_mpc_yawrauto_acc ); - 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 */ + math::WelfordMean _sample_interval_s{}; + + AlphaFilter _vel_xy_lp_filter{}; + AlphaFilter _vel_z_lp_filter{}; + + math::NotchFilter _vel_xy_notch_filter{}; + math::NotchFilter _vel_z_notch_filter{}; + + AlphaFilter _vel_deriv_xy_lp_filter{}; + AlphaFilter _vel_deriv_z_lp_filter{}; GotoControl _goto_control; ///< class for handling smooth goto position setpoints PositionControl _control; ///< class for core PID position control @@ -224,7 +238,7 @@ private: /** * Check for validity of positon/velocity states. */ - PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos); + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const float dt_s); /** * Generate setpoint to bridge no executable setpoint being available. diff --git a/src/modules/mc_pos_control/multicopter_position_control_params.c b/src/modules/mc_pos_control/multicopter_position_control_params.c index a8c00fb167..94435b1b33 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_params.c @@ -75,11 +75,56 @@ PARAM_DEFINE_INT32(MPC_USE_HTE, 1); PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f); /** - * Numerical velocity derivative low pass cutoff frequency + * Velocity low pass cutoff frequency + * + * A value of 0 disables the filter. * * @unit Hz * @min 0 - * @max 10 + * @max 50 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_LP, 0.0f); + +/** + * Velocity notch filter frequency + * + * The center frequency for the 2nd order notch filter on the velocity. + * A value of 0 disables the filter. + * + * @unit Hz + * @min 0 + * @max 50 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_NF_FRQ, 0.0f); + +/** + * Velocity notch filter bandwidth + * + * A value of 0 disables the filter. + * + * @unit Hz + * @min 0 + * @max 50 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_NF_BW, 5.0f); + +/** + * Velocity derivative low pass cutoff frequency + * + * A value of 0 disables the filter. + * + * @unit Hz + * @min 0 + * @max 50 * @decimal 1 * @increment 0.5 * @group Multicopter Position Control