dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

atune: use butterworth 1st order hpf 

Equations from: R. Allred, Digital Filters for Everyone
This commit is contained in:
bresch 2023-12-20 17:37:12 +01:00
parent 1b4815cd98
commit 825c11fc8a
3 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/system_identification/system_identification.cpp
View File

@ -79,8 +79,8 @@ void SystemIdentification::updateFilters(float u, float y)
const float u_lpf = _u_lpf.apply(u);
const float y_lpf = _y_lpf.apply(y);
_u_hpf = _alpha_hpf * _u_hpf + _alpha_hpf * (u_lpf - _u_prev);
_y_hpf = _alpha_hpf * _y_hpf + _alpha_hpf * (y_lpf - _y_prev);
_u_hpf = u_lpf - _u_prev - (_gamma_hpf - 1.f) * _u_hpf;
_y_hpf = y_lpf - _y_prev - (_gamma_hpf - 1.f) * _y_hpf;
_u_prev = u_lpf;
_y_prev = y_lpf;

4
src/lib/system_identification/system_identification.hpp
View File

@ -70,7 +70,7 @@ public:
_u_lpf.set_cutoff_frequency(sample_freq, cutoff);
_y_lpf.set_cutoff_frequency(sample_freq, cutoff);
}
void setHpfCutoffFrequency(float sample_freq, float cutoff) { _alpha_hpf = sample_freq / (sample_freq + 2.f * M_PI_F * cutoff); }
void setHpfCutoffFrequency(float sample_freq, float cutoff) { _gamma_hpf = tanf(M_PI_F * cutoff / sample_freq); }
void setForgettingFactor(float time_constant, float dt) { _rls.setForgettingFactor(time_constant, dt); }
void setFitnessLpfTimeConstant(float time_constant, float dt)
@ -88,7 +88,7 @@ private:
math::LowPassFilter2p<float> _y_lpf{400.f, 30.f};
//TODO: replace by HighPassFilter class
float _alpha_hpf{0.f};
float _gamma_hpf{0.f};
float _u_hpf{0.f};
float _y_hpf{0.f};

2
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
View File

@ -241,7 +241,7 @@ void McAutotuneAttitudeControl::checkFilters()
const float filter_rate_hz = 1.f / _filter_dt;
_sys_id.setLpfCutoffFrequency(filter_rate_hz, _param_imu_gyro_cutoff.get());
_sys_id.setHpfCutoffFrequency(filter_rate_hz, .5f);
_sys_id.setHpfCutoffFrequency(filter_rate_hz, .1f);
// Set the model sampling time depending on the gyro cutoff frequency
// as this is a good indicator of the maximum control loop bandwidth