diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 15cab53e3c..b30ea4d0f9 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -53,9 +53,13 @@ VehicleAngularVelocity::~VehicleAngularVelocity() Stop(); perf_free(_filter_reset_perf); - perf_free(_dynamic_notch_filter_update_perf); perf_free(_selection_changed_perf); - perf_free(_dynamic_notch_filter_perf); +#if !defined(CONSTRAINED_FLASH) + 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_perf); +#endif // CONSTRAINED_FLASH } bool VehicleAngularVelocity::Start() @@ -158,7 +162,10 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons _lp_filter_acceleration[axis].reset(angular_acceleration(axis)); // dynamic notch filters, first disable, then force update (if available) + DisableDynamicNotchEscRpm(); DisableDynamicNotchFFT(); + + UpdateDynamicNotchEscRpm(true); UpdateDynamicNotchFFT(true); } @@ -214,6 +221,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) _reset_filters = true; _bias.zero(); _fifo_available = true; + _last_scale = 0.f; perf_count(_selection_changed_perf); @@ -239,6 +247,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) _reset_filters = true; _bias.zero(); _fifo_available = false; + _last_scale = 1.f; perf_count(_selection_changed_perf); @@ -295,26 +304,97 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) } } -void VehicleAngularVelocity::DisableDynamicNotchFFT() +void VehicleAngularVelocity::DisableDynamicNotchEscRpm() { #if !defined(CONSTRAINED_FLASH) // device id mismatch, disable all - for (auto &dnf : _dynamic_notch_filter) { + for (auto &dnf : _dynamic_notch_filter_esc_rpm) { for (int axis = 0; axis < 3; axis++) { dnf[axis].setParameters(0, 0, 0); } } - _dynamic_notch_available = false; + _dynamic_notch_esc_rpm_available = false; +#endif // !CONSTRAINED_FLASH +} + +void VehicleAngularVelocity::DisableDynamicNotchFFT() +{ +#if !defined(CONSTRAINED_FLASH) + + // device id mismatch, disable all + for (auto &dnf : _dynamic_notch_filter_fft) { + for (int axis = 0; axis < 3; axis++) { + dnf[axis].setParameters(0, 0, 0); + } + } + + _dynamic_notch_fft_available = false; +#endif // !CONSTRAINED_FLASH +} + +void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) +{ +#if !defined(CONSTRAINED_FLASH) + const bool enabled = _param_imu_gyro_dyn_nf.get() & 0b01; + + if (enabled && (_esc_status_sub.updated() || force)) { + _dynamic_notch_esc_rpm_available = false; + + esc_status_s esc_status; + + if (_esc_status_sub.copy(&esc_status)) { + for (size_t i = 0; i < MAX_NUM_ESC_RPM; i++) { + static constexpr int32_t MIN_ESC_RPM = 20 * 60; // 20 Hz safe minimum limit TODO: configurable + + if ((esc_status.esc[i].timestamp != 0) && ((_timestamp_sample_last - esc_status.esc[i].timestamp) < 1_s) + && (esc_status.esc[i].esc_rpm > MIN_ESC_RPM)) { + + // TODO: rotor frequency, blade pass frequency, harmonics + const float frequency_hz = static_cast(esc_status.esc[i].esc_rpm) / 60.f; + + for (int axis = 0; axis < 3; axis++) { + auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis]; + const float change_percent = fabsf(dnf.getNotchFreq() - frequency_hz) / frequency_hz; + + if (change_percent > 0.001f) { + // peak frequency changed by at least 0.1% + dnf.setParameters(_filter_sample_rate_hz, frequency_hz, 1.f); // TODO: configurable bandwidth + + // only reset if there's sufficient change (> 1%) + if ((change_percent > 0.01f) && (_last_scale > 0.f)) { + dnf.reset(_angular_velocity(axis) / _last_scale); + } + + perf_count(_dynamic_notch_filter_esc_rpm_update_perf); + } + } + + _dynamic_notch_esc_rpm_available = true; + + } else { + // disable this notch filter + for (int axis = 0; axis < 3; axis++) { + auto &dnf = _dynamic_notch_filter_esc_rpm[i][axis]; + dnf.setParameters(0, 0, 0); + } + } + } + } + } + #endif // !CONSTRAINED_FLASH } void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) { #if !defined(CONSTRAINED_FLASH) + const bool enabled = _param_imu_gyro_dyn_nf.get() & 0b10; + + if (enabled && (_sensor_gyro_fft_sub.updated() || force)) { + _dynamic_notch_fft_available = false; - if (_param_imu_gyro_dyn_nf.get() && (_sensor_gyro_fft_sub.updated() || force)) { sensor_gyro_fft_s sensor_gyro_fft; if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) && (sensor_gyro_fft.device_id == _selected_sensor_device_id) @@ -324,7 +404,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) for (int axis = 0; axis < 3; axis++) { for (int i = 0; i < MAX_NUM_FFT_PEAKS; i++) { - auto &dnf = _dynamic_notch_filter[i][axis]; + auto &dnf = _dynamic_notch_filter_fft[i][axis]; const float &peak_freq = peak_frequencies[axis][i]; if (PX4_ISFINITE(peak_freq) && (peak_freq > 1.f)) { @@ -335,14 +415,14 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) dnf.setParameters(_filter_sample_rate_hz, peak_freq, sensor_gyro_fft.resolution_hz); // only reset if there's sufficient change (> 1%) - if ((change_percent > 0.01f) && (_fifo_last_scale > 0.f)) { - dnf.reset(_angular_velocity(axis) / _fifo_last_scale); + if ((change_percent > 0.01f) && (_last_scale > 0.f)) { + dnf.reset(_angular_velocity(axis) / _last_scale); } - perf_count(_dynamic_notch_filter_update_perf); + perf_count(_dynamic_notch_filter_fft_update_perf); } - _dynamic_notch_available = true; + _dynamic_notch_fft_available = true; } else { // disable this notch filter @@ -383,13 +463,13 @@ void VehicleAngularVelocity::Run() const float dt_s = sensor_fifo_data.dt * 1e-6f; const enum Rotation fifo_rotation = static_cast(sensor_fifo_data.rotation); - if (_reset_filters || (fabsf(sensor_fifo_data.scale - _fifo_last_scale) > FLT_EPSILON)) { + if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) { if (UpdateSampleRate()) { // in FIFO mode the unscaled raw data is filtered _angular_velocity_prev = _angular_velocity / sensor_fifo_data.scale; ResetFilters(_angular_velocity_prev, _angular_acceleration / sensor_fifo_data.scale); - _fifo_last_scale = sensor_fifo_data.scale; + _last_scale = sensor_fifo_data.scale; } if (_reset_filters) { @@ -397,6 +477,7 @@ void VehicleAngularVelocity::Run() } } + UpdateDynamicNotchEscRpm(); UpdateDynamicNotchFFT(); Vector3f angular_velocity_unscaled; @@ -414,17 +495,30 @@ void VehicleAngularVelocity::Run() #if !defined(CONSTRAINED_FLASH) - // Apply dynamic notch filter from FFT - if (_dynamic_notch_available) { - perf_begin(_dynamic_notch_filter_perf); + // Apply dynamic notch filter from ESC RPM + if (_dynamic_notch_esc_rpm_available) { + perf_begin(_dynamic_notch_filter_esc_rpm_perf); - for (auto &dnf : _dynamic_notch_filter) { + for (auto &dnf : _dynamic_notch_filter_esc_rpm) { if (dnf[axis].getNotchFreq() > 0.f) { dnf[axis].applyDF1(data, N); } } - perf_end(_dynamic_notch_filter_perf); + perf_end(_dynamic_notch_filter_esc_rpm_perf); + } + + // Apply dynamic notch filter from FFT + if (_dynamic_notch_fft_available) { + perf_begin(_dynamic_notch_filter_fft_perf); + + for (auto &dnf : _dynamic_notch_filter_fft) { + if (dnf[axis].getNotchFreq() > 0.f) { + dnf[axis].applyDF1(data, N); + } + } + + perf_end(_dynamic_notch_filter_fft_perf); } #endif // !CONSTRAINED_FLASH @@ -488,10 +582,43 @@ void VehicleAngularVelocity::Run() } } + UpdateDynamicNotchEscRpm(); + UpdateDynamicNotchFFT(); + // Apply calibration, rotation, and correct for in-run bias errors Vector3f angular_velocity{_calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias}; for (int axis = 0; axis < 3; axis++) { +#if !defined(CONSTRAINED_FLASH) + + // Apply dynamic notch filter from ESC RPM + if (_dynamic_notch_esc_rpm_available) { + perf_begin(_dynamic_notch_filter_esc_rpm_perf); + + for (auto &dnf : _dynamic_notch_filter_esc_rpm) { + if (dnf[axis].getNotchFreq() > 0.f) { + dnf[axis].applyDF1(&angular_velocity(axis), 1); + } + } + + perf_end(_dynamic_notch_filter_esc_rpm_perf); + } + + // Apply dynamic notch filter from FFT + if (_dynamic_notch_fft_available) { + perf_begin(_dynamic_notch_filter_fft_perf); + + for (auto &dnf : _dynamic_notch_filter_fft) { + if (dnf[axis].getNotchFreq() > 0.f) { + dnf[axis].applyDF1(&angular_velocity(axis), 1); + } + } + + perf_end(_dynamic_notch_filter_fft_perf); + } + +#endif // !CONSTRAINED_FLASH + // Apply general notch filter (IMU_GYRO_NF_FREQ) _notch_filter_velocity[axis].apply(&angular_velocity(axis), 1); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index e2b32422d0..18c8d4c510 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -74,12 +75,14 @@ public: private: void Run() override; + void DisableDynamicNotchEscRpm(); void DisableDynamicNotchFFT(); void ParametersUpdate(bool force = false); void Publish(const hrt_abstime ×tamp_sample); void ResetFilters(const matrix::Vector3f &angular_velocity, const matrix::Vector3f &angular_acceleration); void SensorBiasUpdate(bool force = false); bool SensorSelectionUpdate(bool force = false); + void UpdateDynamicNotchEscRpm(bool force = false); void UpdateDynamicNotchFFT(bool force = false); bool UpdateSampleRate(); @@ -91,6 +94,7 @@ private: uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; #if !defined(CONSTRAINED_FLASH) + uORB::Subscription _esc_status_sub {ORB_ID(esc_status)}; uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)}; #endif // !CONSTRAINED_FLASH @@ -122,11 +126,20 @@ private: math::NotchFilterArray _notch_filter_velocity[3] {}; #if !defined(CONSTRAINED_FLASH) + static constexpr int MAX_NUM_ESC_RPM = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]); static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof( sensor_gyro_fft_s::peak_frequencies_x[0]); - math::NotchFilterArray _dynamic_notch_filter[MAX_NUM_FFT_PEAKS][3] {}; - bool _dynamic_notch_available{false}; + math::NotchFilterArray _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][3] {}; + math::NotchFilterArray _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {}; + + perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM update")}; + perf_counter_t _dynamic_notch_filter_fft_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update")}; + perf_counter_t _dynamic_notch_filter_esc_rpm_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter")}; + perf_counter_t _dynamic_notch_filter_fft_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter")}; + + bool _dynamic_notch_esc_rpm_available{false}; + bool _dynamic_notch_fft_available{false}; #endif // !CONSTRAINED_FLASH // angular acceleration filter @@ -134,23 +147,23 @@ private: uint32_t _selected_sensor_device_id{0}; - float _fifo_last_scale{0}; + float _last_scale{0.f}; bool _reset_filters{true}; bool _fifo_available{false}; perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro filter reset")}; - perf_counter_t _dynamic_notch_filter_update_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter update")}; perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro selection changed")}; - perf_counter_t _dynamic_notch_filter_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch filter")}; DEFINE_PARAMETERS( +#if !defined(CONSTRAINED_FLASH) + (ParamInt) _param_imu_gyro_dyn_nf, +#endif // !CONSTRAINED_FLASH (ParamFloat) _param_imu_gyro_cutoff, (ParamFloat) _param_imu_gyro_nf_freq, (ParamFloat) _param_imu_gyro_nf_bw, (ParamInt) _param_imu_gyro_ratemax, - (ParamFloat) _param_imu_dgyro_cutoff, - (ParamBool) _param_imu_gyro_dyn_nf + (ParamFloat) _param_imu_dgyro_cutoff ) }; diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index eeac87b81e..41e95f983f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -126,10 +126,11 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f); * IMU gyro dynamic notch filtering * * Enable bank of dynamically updating notch filters. -* Requires onboard FFT (IMU_GYRO_FFT_EN). -* -* @boolean -* @reboot_required true +* Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN). * @group Sensors +* @min 0 +* @max 2 +* @bit 0 ESC RPM +* @bit 1 FFT */ PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0);