zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

sensors/vehicle_angular_velocity: add new parameter for ESC RPM notch filter BW

master
Daniel Agar 3 years ago
parent
02709fcfab
commit
376b72fb2f
4 changed files with 53 additions and 22 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      posix-configs/SITL/init/test/test_imu_filtering
  2. 44
      src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
  3. 11
      src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
  4. 14
      src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

6
posix-configs/SITL/init/test/test_imu_filtering
Unescape View File

@ -20,9 +20,9 @@ param set IMU_GYRO_FFT_MAX 1000 @@ -20,9 +20,9 @@ param set IMU_GYRO_FFT_MAX 1000
param set IMU_GYRO_FFT_LEN 512
# dynamic notches ESC/FFT/both
#param set IMU_GYRO_DYN_NF 1
#param set IMU_GYRO_DYN_NF 2
param set IMU_GYRO_DYN_NF 3
#param set IMU_GYRO_DNF_EN 1
#param set IMU_GYRO_DNF_EN 2
param set IMU_GYRO_DNF_EN 3
# test values
param set IMU_GYRO_CUTOFF 60

44
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
Unescape View File

@ -55,9 +55,14 @@ VehicleAngularVelocity::~VehicleAngularVelocity() @@ -55,9 +55,14 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
perf_free(_filter_reset_perf);
perf_free(_selection_changed_perf);
#if !defined(CONSTRAINED_FLASH)
perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
perf_free(_dynamic_notch_filter_esc_rpm_reset_perf);
perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
perf_free(_dynamic_notch_filter_fft_update_perf);
perf_free(_dynamic_notch_filter_esc_rpm_perf);
perf_free(_dynamic_notch_filter_fft_disable_perf);
perf_free(_dynamic_notch_filter_fft_reset_perf);
perf_free(_dynamic_notch_filter_fft_update_perf);
perf_free(_dynamic_notch_filter_fft_perf);
#endif // CONSTRAINED_FLASH
}
@ -313,13 +318,13 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) @@ -313,13 +318,13 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
#if !defined(CONSTRAINED_FLASH)
if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm) {
if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) {
if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) {
if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) {
_dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM disable");
_dynamic_notch_filter_esc_rpm_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM reset");
_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
MODULE_NAME": gyro dynamic notch filter ESC RPM update");
}
if (_dynamic_notch_filter_esc_rpm_perf == nullptr) {
_dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter");
}
@ -327,12 +332,11 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) @@ -327,12 +332,11 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
DisableDynamicNotchEscRpm();
}
if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) {
if (_dynamic_notch_filter_fft_update_perf == nullptr) {
if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) {
if (_dynamic_notch_filter_fft_disable_perf == nullptr) {
_dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable");
_dynamic_notch_filter_fft_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT reset");
_dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update");
}
if (_dynamic_notch_filter_fft_perf == nullptr) {
_dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter");
}
@ -392,6 +396,7 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm() @@ -392,6 +396,7 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
}
_esc_available.set(esc, false);
perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
}
}
@ -413,6 +418,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() @@ -413,6 +418,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
}
_dynamic_notch_fft_available = false;
perf_count(_dynamic_notch_filter_fft_disable_perf);
}
#endif // !CONSTRAINED_FLASH
@ -421,7 +427,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() @@ -421,7 +427,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
{
#if !defined(CONSTRAINED_FLASH)
const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm;
const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm;
if (enabled && (_esc_status_sub.updated() || force)) {
@ -454,10 +460,9 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) @@ -454,10 +460,9 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
// update filter parameters if frequency changed or forced
if (force || reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > FLT_EPSILON)) {
static constexpr float ESC_NOTCH_BW_HZ = 8.f; // TODO: configurable bandwidth
// force reset if the notch frequency jumps significantly
if (!reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > ESC_NOTCH_BW_HZ)) {
if (!reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > _param_imu_gyro_dnf_bw.get())) {
reset = true;
}
@ -465,7 +470,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) @@ -465,7 +470,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
const float frequency_hz = esc_hz * (harmonic + 1);
for (int axis = 0; axis < 3; axis++) {
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(_filter_sample_rate_hz, frequency_hz, ESC_NOTCH_BW_HZ);
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(_filter_sample_rate_hz, frequency_hz,
_param_imu_gyro_dnf_bw.get());
}
}
@ -480,6 +486,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) @@ -480,6 +486,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].reset(reset_angular_velocity(axis));
}
}
perf_count(_dynamic_notch_filter_esc_rpm_reset_perf);
}
_dynamic_notch_esc_rpm_available = true;
@ -491,6 +499,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) @@ -491,6 +499,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
// if this ESC was previously available disable all notch filters if forced or timeout
_esc_available.set(esc, false);
perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
for (int axis = 0; axis < 3; axis++) {
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(0, 0, 0);
@ -510,7 +520,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) @@ -510,7 +520,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
{
#if !defined(CONSTRAINED_FLASH)
const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT;
const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::FFT;
if (enabled && (_sensor_gyro_fft_sub.updated() || force)) {
@ -556,6 +566,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) @@ -556,6 +566,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
if (force || reset || (notch_freq_diff > bandwidth)) {
const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
nf.reset(reset_angular_velocity(axis));
perf_count(_dynamic_notch_filter_fft_reset_perf);
}
_dynamic_notch_fft_available = true;
@ -564,6 +575,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) @@ -564,6 +575,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
// disable this notch filter (if it isn't already)
if (force || !reset) {
nf.setParameters(0, 0, 0);
perf_count(_dynamic_notch_filter_fft_disable_perf);
}
}
}

11
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
Unescape View File

@ -157,8 +157,14 @@ private: @@ -157,8 +157,14 @@ private:
hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESC_RPM] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_reset_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_esc_rpm_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_reset_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_perf{nullptr};
bool _dynamic_notch_esc_rpm_available{false};
@ -178,7 +184,8 @@ private: @@ -178,7 +184,8 @@ private:
DEFINE_PARAMETERS(
#if !defined(CONSTRAINED_FLASH)
(ParamInt<px4::params::IMU_GYRO_DYN_NF>) _param_imu_gyro_dyn_nf,
(ParamInt<px4::params::IMU_GYRO_DNF_EN>) _param_imu_gyro_dnf_en,
(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,

14
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
Unescape View File

@ -136,4 +136,16 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); @@ -136,4 +136,16 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f);
* @bit 0 ESC RPM
* @bit 1 FFT
*/
PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0);
PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0);
/**
* IMU gyro ESC notch filter bandwidth
*
* Bandwidth per notch filter when using dynamic notch filtering with ESC RPM.
*
* @group Sensors
* @category Developer
* @min 5
* @max 30
*/
PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_BW, 8.f);

Write Preview
Loading…
Cancel
Save