fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 01:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

[Multirotor] add yaw torque low pass filter (#24173)

Browse Source 
co-authored-by: danielmellinger <107884356+danielmellinger@users.noreply.github.com>
co-authored-by: Eric Katzfey <eric.katzfey@modalai.com>
This commit is contained in:
Mathieu Bresciani 2025-01-15 14:02:28 +01:00 committed by GitHub
parent 974446c0e8
commit 8ecb76aba2
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 34 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/lib/mathlib/math/filter/AlphaFilter.hpp
View File

@ -89,6 +89,16 @@ public:
return true;
}
void setCutoffFreq(float cutoff_freq)
{
if (cutoff_freq > FLT_EPSILON) {
_time_constant = 1.f / (M_TWOPI_F * cutoff_freq);
} else {
_time_constant = 0.f;
}
}
/**
* Set filter parameter alpha directly without time abstraction
*

7
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -95,6 +95,8 @@ MulticopterRateControl::parameters_updated()
// manual rate control acro mode rate limits
_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
radians(_param_mc_acro_y_max.get()));
_output_lpf_yaw.setCutoffFreq(_param_mc_yaw_tq_cutoff.get());
}
void
@ -214,9 +216,12 @@ MulticopterRateControl::Run()
}
// run rate controller
const Vector3f torque_setpoint =
Vector3f torque_setpoint =
_rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed);
// apply low-pass filtering on yaw axis to reduce high frequency torque caused by rotor acceleration
torque_setpoint(2) = _output_lpf_yaw.update(torque_setpoint(2), dt);
// publish rate controller status
rate_ctrl_status_s rate_ctrl_status{};
_rate_control.getRateControlStatus(rate_ctrl_status);

4
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -34,6 +34,7 @@
#pragma once
#include <lib/rate_control/rate_control.hpp>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
@ -129,6 +130,8 @@ private:
float _energy_integration_time{0.0f};
float _control_energy[4] {};
AlphaFilter<float> _output_lpf_yaw;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,
@ -150,6 +153,7 @@ private:
(ParamFloat<px4::params::MC_YAWRATE_D>) _param_mc_yawrate_d,
(ParamFloat<px4::params::MC_YAWRATE_FF>) _param_mc_yawrate_ff,
(ParamFloat<px4::params::MC_YAWRATE_K>) _param_mc_yawrate_k,
(ParamFloat<px4::params::MC_YAW_TQ_CUTOFF>) _param_mc_yaw_tq_cutoff,
(ParamFloat<px4::params::MC_ACRO_R_MAX>) _param_mc_acro_r_max,
(ParamFloat<px4::params::MC_ACRO_P_MAX>) _param_mc_acro_p_max,

14
src/modules/mc_rate_control/mc_rate_control_params.c
View File

@ -292,3 +292,17 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f);
* @group Multicopter Rate Control
*/
PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0);
/**
* Low pass filter cutoff frequency for yaw torque setpoint
*
* Reduces vibrations by lowering high frequency torque caused by rotor acceleration.
* 0 disables the filter
*
* @min 0
* @max 10
* @unit Hz
* @decimal 3
* @group Multicopter Rate Control
*/
PARAM_DEFINE_FLOAT(MC_YAW_TQ_CUTOFF, 2.f);