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

sensors: add 2nd static notch and migrate existing parameters 

- new static notch filter configured via IMU_GYRO_NF1_FRQ and IMU_GYRO_NF1_BW
 - existing notch parameters IMU_GYRO_NF_FREQ and IMU_GYRO_NF_BW become
IMU_GYRO_NF0_FRQ and IMU_GYRO_NF0_BW
This commit is contained in:
Daniel Agar 2022-03-15 16:46:05 -04:00
parent afeab9587e
commit 475bd42ab8
5 changed files with 99 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
posix-configs/SITL/init/test/test_imu_filtering
View File

@ -28,8 +28,8 @@ param set IMU_GYRO_DNF_EN 3
# test values
param set IMU_GYRO_CUTOFF 60
param set IMU_DGYRO_CUTOFF 40
#param set IMU_GYRO_NF_FREQ 60
#param set IMU_GYRO_NF_BW 5
#param set IMU_GYRO_NF0_FRQ 60
#param set IMU_GYRO_NF0_BW 5
# log nearly everything
param set SDLOG_PROFILE 859

15
src/lib/parameters/param_translation.cpp
View File

@ -163,5 +163,20 @@ bool param_modify_on_import(bson_node_t node)
}
}
// 2022-03-15: translate notch filter IMU_GYRO_NF_FREQ to IMU_GYRO_NF0_FRQ and IMU_GYRO_NF_BW -> IMU_GYRO_NF0_BW
{
if (strcmp("IMU_GYRO_NF_FREQ", node->name) == 0) {
strcpy(node->name, "IMU_GYRO_NF0_FRQ");
PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_FREQ", "IMU_GYRO_NF0_FRQ");
return true;
}
if (strcmp("IMU_GYRO_NF_BW", node->name) == 0) {
strcpy(node->name, "IMU_GYRO_NF0_BW");
PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_BW", "IMU_GYRO_NF0_BW");
return true;
}
}
return false;
}

51
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -167,10 +167,15 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us)
_lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get());
_lp_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis));
// angular velocity notch
_notch_filter_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf_freq.get(),
_param_imu_gyro_nf_bw.get());
_notch_filter_velocity[axis].reset();
// angular velocity notch 0
_notch_filter0_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf0_frq.get(),
_param_imu_gyro_nf0_bw.get());
_notch_filter0_velocity[axis].reset();
// angular velocity notch 1
_notch_filter1_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf1_frq.get(),
_param_imu_gyro_nf1_bw.get());
_notch_filter1_velocity[axis].reset();
// angular acceleration low pass
if ((_param_imu_dgyro_cutoff.get() > 0.f)
@ -360,11 +365,13 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
const bool nf_enabled_prev = (_param_imu_gyro_nf_freq.get() > 0.f) && (_param_imu_gyro_nf_bw.get() > 0.f);
const bool nf0_enabled_prev = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f);
const bool nf1_enabled_prev = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f);
updateParams();
const bool nf_enabled = (_param_imu_gyro_nf_freq.get() > 0.f) && (_param_imu_gyro_nf_bw.get() > 0.f);
const bool nf0_enabled = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f);
const bool nf1_enabled = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f);
_calibration.ParametersUpdate();
@ -376,12 +383,23 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
}
}
// gyro notch filter frequency or bandwidth changed
for (auto &nf : _notch_filter_velocity) {
const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f);
const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f);
// gyro notch filter 0 frequency or bandwidth changed
for (auto &nf : _notch_filter0_velocity) {
const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf0_frq.get()) > 0.01f);
const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf0_bw.get()) > 0.01f);
if ((nf_enabled_prev != nf_enabled) || (nf_enabled && (nf_freq_changed || nf_bw_changed))) {
if ((nf0_enabled_prev != nf0_enabled) || (nf0_enabled && (nf_freq_changed || nf_bw_changed))) {
_reset_filters = true;
break;
}
}
// gyro notch filter 1 frequency or bandwidth changed
for (auto &nf : _notch_filter1_velocity) {
const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf1_frq.get()) > 0.01f);
const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf1_bw.get()) > 0.01f);
if ((nf1_enabled_prev != nf1_enabled) || (nf1_enabled && (nf_freq_changed || nf_bw_changed))) {
_reset_filters = true;
break;
}
@ -695,9 +713,14 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
#endif // !CONSTRAINED_FLASH
// Apply general notch filter (IMU_GYRO_NF_FREQ)
if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
_notch_filter_velocity[axis].applyArray(data, N);
// Apply general notch filter 0 (IMU_GYRO_NF0_FRQ)
if (_notch_filter0_velocity[axis].getNotchFreq() > 0.f) {
_notch_filter0_velocity[axis].applyArray(data, N);
}
// Apply general notch filter 1 (IMU_GYRO_NF1_FRQ)
if (_notch_filter1_velocity[axis].getNotchFreq() > 0.f) {
_notch_filter1_velocity[axis].applyArray(data, N);
}
// Apply general low-pass filter (IMU_GYRO_CUTOFF)

9
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -132,7 +132,8 @@ private:
// angular velocity filters
math::LowPassFilter2p<float> _lp_filter_velocity[3] {};
math::NotchFilter<float> _notch_filter_velocity[3] {};
math::NotchFilter<float> _notch_filter0_velocity[3] {};
math::NotchFilter<float> _notch_filter1_velocity[3] {};
#if !defined(CONSTRAINED_FLASH)
@ -188,8 +189,10 @@ private:
(ParamFloat<px4::params::IMU_GYRO_DNF_BW>) _param_imu_gyro_dnf_bw,
#endif // !CONSTRAINED_FLASH
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
(ParamFloat<px4::params::IMU_GYRO_NF0_FRQ>) _param_imu_gyro_nf0_frq,
(ParamFloat<px4::params::IMU_GYRO_NF0_BW>) _param_imu_gyro_nf0_bw,
(ParamFloat<px4::params::IMU_GYRO_NF1_FRQ>) _param_imu_gyro_nf1_frq,
(ParamFloat<px4::params::IMU_GYRO_NF1_BW>) _param_imu_gyro_nf1_bw,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
)

43
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
View File

@ -38,7 +38,7 @@
* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency.
* This only affects the signal sent to the controllers, not the estimators.
* Applies to both angular velocity and angular acceleration sent to the controllers.
* See "IMU_GYRO_NF_BW" to set the bandwidth of the filter.
* See "IMU_GYRO_NF0_BW" to set the bandwidth of the filter.
*
* A value of 0 disables the filter.
*
@ -48,13 +48,13 @@
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_FRQ, 0.0f);
/**
* Notch filter bandwidth for gyro
*
* The frequency width of the stop band for the 2nd order notch filter on the primary gyro.
* See "IMU_GYRO_NF_FREQ" to activate the filter and to set the notch frequency.
* See "IMU_GYRO_NF0_FRQ" to activate the filter and to set the notch frequency.
* Applies to both angular velocity and angular acceleration sent to the controllers.
*
* @min 0
@ -63,7 +63,42 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_FREQ, 0.0f);
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_BW, 20.0f);
/**
* Notch filter 2 frequency for gyro
*
* The center frequency for the 2nd order notch filter on the primary gyro.
* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency.
* This only affects the signal sent to the controllers, not the estimators.
* Applies to both angular velocity and angular acceleration sent to the controllers.
* See "IMU_GYRO_NF1_BW" to set the bandwidth of the filter.
*
* A value of 0 disables the filter.
*
* @min 0
* @max 1000
* @unit Hz
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f);
/**
* Notch filter 1 bandwidth for gyro
*
* The frequency width of the stop band for the 2nd order notch filter on the primary gyro.
* See "IMU_GYRO_NF1_FRQ" to activate the filter and to set the notch frequency.
* Applies to both angular velocity and angular acceleration sent to the controllers.
*
* @min 0
* @max 100
* @unit Hz
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f);
/**
* Low pass filter cutoff frequency for gyro