diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index b30ea4d0f9..62ffed5eec 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -301,6 +301,31 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) break; } } + +#if !defined(CONSTRAINED_FLASH) + + if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm) { + if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) { + _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"); + } + } + + if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) { + if (_dynamic_notch_filter_fft_update_perf == nullptr) { + _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"); + } + } + +#endif // !CONSTRAINED_FLASH } } @@ -337,7 +362,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) { #if !defined(CONSTRAINED_FLASH) - const bool enabled = _param_imu_gyro_dyn_nf.get() & 0b01; + const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm; if (enabled && (_esc_status_sub.updated() || force)) { _dynamic_notch_esc_rpm_available = false; @@ -390,7 +415,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) { #if !defined(CONSTRAINED_FLASH) - const bool enabled = _param_imu_gyro_dyn_nf.get() & 0b10; + const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT; if (enabled && (_sensor_gyro_fft_sub.updated() || force)) { _dynamic_notch_fft_available = false; @@ -400,7 +425,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) && (sensor_gyro_fft.device_id == _selected_sensor_device_id) && (fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) < 0.05f)) { - float *peak_frequencies[] { sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z}; + float *peak_frequencies[] {sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z}; for (int axis = 0; axis < 3; axis++) { for (int i = 0; i < MAX_NUM_FFT_PEAKS; i++) { diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 18c8d4c510..39ca082d01 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -126,6 +126,12 @@ private: math::NotchFilterArray _notch_filter_velocity[3] {}; #if !defined(CONSTRAINED_FLASH) + + enum DynamicNotch { + EscRpm = 1, + FFT = 2, + }; + 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]); @@ -133,10 +139,10 @@ private: 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")}; + 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_perf{nullptr}; + perf_counter_t _dynamic_notch_filter_fft_perf{nullptr}; bool _dynamic_notch_esc_rpm_available{false}; bool _dynamic_notch_fft_available{false}; 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 41e95f983f..1a87b2ac1d 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -129,7 +129,7 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f); * Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN). * @group Sensors * @min 0 -* @max 2 +* @max 3 * @bit 0 ESC RPM * @bit 1 FFT */