From ae010ea55cfea0d5b49209dc3028df63f8d2a1c0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 22 Mar 2021 12:30:21 -0400 Subject: [PATCH] sensors/vehicle_angular_velocity: unify filtering for both FIFO and regular use cases --- .../VehicleAngularVelocity.cpp | 293 +++++++++--------- .../VehicleAngularVelocity.hpp | 11 +- 2 files changed, 156 insertions(+), 148 deletions(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 97372a7e62..c1822bcb66 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -146,8 +146,11 @@ bool VehicleAngularVelocity::UpdateSampleRate() return false; } -void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, const Vector3f &angular_acceleration) +void VehicleAngularVelocity::ResetFilters() { + const Vector3f angular_velocity{GetResetAngularVelocity()}; + const Vector3f angular_acceleration{GetResetAngularAcceleration()}; + for (int axis = 0; axis < 3; axis++) { // angular velocity low pass _lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get()); @@ -161,15 +164,17 @@ void VehicleAngularVelocity::ResetFilters(const Vector3f &angular_velocity, cons // angular acceleration low pass _lp_filter_acceleration[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()); _lp_filter_acceleration[axis].reset(angular_acceleration(axis)); - - // dynamic notch filters, first disable, then force update (if available) - DisableDynamicNotchEscRpm(); - DisableDynamicNotchFFT(); - - UpdateDynamicNotchEscRpm(true); - UpdateDynamicNotchFFT(true); } + // dynamic notch filters, first disable, then force update (if available) + DisableDynamicNotchEscRpm(); + DisableDynamicNotchFFT(); + + UpdateDynamicNotchEscRpm(true); + UpdateDynamicNotchFFT(true); + + _angular_velocity_prev = angular_velocity; + _reset_filters = false; perf_count(_filter_reset_perf); } @@ -314,6 +319,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) 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"); } + + } else { + DisableDynamicNotchEscRpm(); } if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) { @@ -324,6 +332,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) if (_dynamic_notch_filter_fft_perf == nullptr) { _dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter"); } + + } else { + DisableDynamicNotchFFT(); } #endif // !CONSTRAINED_FLASH @@ -332,11 +343,27 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const { - if (_fifo_available && (_last_scale > 0.f)) { + if ((_last_publish != 0) && (_last_scale > 0.f) + && PX4_ISFINITE(_angular_velocity(0)) + && PX4_ISFINITE(_angular_velocity(1)) + && PX4_ISFINITE(_angular_velocity(2))) { + // angular velocity filtering is performed on raw unscaled data + // start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection) return _calibration.Uncorrect(_angular_velocity + _bias) / _last_scale; + } - } else if (!_fifo_available) { - return _angular_velocity; + return Vector3f{0.f, 0.f, 0.f}; +} + +Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const +{ + if ((_last_publish != 0) && (_last_scale > 0.f) + && PX4_ISFINITE(_angular_acceleration(0)) + && PX4_ISFINITE(_angular_acceleration(1)) + && PX4_ISFINITE(_angular_acceleration(2))) { + // angular acceleration filtering is performed on raw unscaled data + // start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection) + return _calibration.rotation().I() * _angular_acceleration / _last_scale; } return Vector3f{0.f, 0.f, 0.f}; @@ -490,6 +517,73 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) #endif // !CONSTRAINED_FLASH } +float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int N) +{ +#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) { + for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { + if (dnf[harmonic][axis].getNotchFreq() > 0.f) { + dnf[harmonic][axis].applyDF1(data, N); + + } else { + break; + } + } + } + + 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 + + // Apply general notch filter (IMU_GYRO_NF_FREQ) + if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) { + _notch_filter_velocity[axis].apply(data, N); + } + + // Apply general low-pass filter (IMU_GYRO_CUTOFF) + _lp_filter_velocity[axis].apply(data, N); + + // return last filtered sample + return data[N - 1]; +} + +float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float data[], int N, float dt_s) +{ + if (N > 0) { + // angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + float delta_velocity_filtered; + + for (int n = 0; n < N; n++) { + const float delta_velocity = (data[n] - _angular_velocity_prev(axis)); + delta_velocity_filtered = _lp_filter_acceleration[axis].apply(delta_velocity); + _angular_velocity_prev(axis) = data[n]; + } + + return delta_velocity_filtered / dt_s; + } + + return 0.f; +} + void VehicleAngularVelocity::Run() { // backup schedule @@ -507,30 +601,26 @@ void VehicleAngularVelocity::Run() sensor_gyro_fifo_s sensor_fifo_data; while (_sensor_fifo_sub.update(&sensor_fifo_data)) { - static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); + const float dt_s = sensor_fifo_data.dt * 1e-6f; + _timestamp_sample_last = sensor_fifo_data.timestamp_sample; - if ((sensor_fifo_data.samples > 0) && (sensor_fifo_data.samples <= FIFO_SIZE_MAX)) { - _timestamp_sample_last = sensor_fifo_data.timestamp_sample; + if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) { + if (UpdateSampleRate()) { + // in FIFO mode the unscaled raw data is filtered + _last_scale = sensor_fifo_data.scale; - const int N = sensor_fifo_data.samples; - const float dt_s = sensor_fifo_data.dt * 1e-6f; - - if (_reset_filters || (fabsf(sensor_fifo_data.scale - _last_scale) > FLT_EPSILON)) { - if (UpdateSampleRate()) { - // in FIFO mode the unscaled raw data is filtered - _last_scale = sensor_fifo_data.scale; - - _angular_velocity_prev = GetResetAngularVelocity(); - Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration / _last_scale}; - - ResetFilters(_angular_velocity_prev, angular_acceleration); - } - - if (_reset_filters) { - continue; // not safe to run until filters configured - } + ResetFilters(); } + if (_reset_filters) { + continue; // not safe to run until filters configured + } + } + + const int N = sensor_fifo_data.samples; + static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); + + if ((N > 0) && (N <= FIFO_SIZE_MAX)) { UpdateDynamicNotchEscRpm(); UpdateDynamicNotchFFT(); @@ -547,74 +637,15 @@ void VehicleAngularVelocity::Run() data[n] = raw_data_array[axis][n]; } -#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) { - for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { - if (dnf[harmonic][axis].getNotchFreq() > 0.f) { - dnf[harmonic][axis].applyDF1(data, N); - - } else { - break; - } - } - } - - 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 - - // Apply general notch filter (IMU_GYRO_NF_FREQ) - if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) { - _notch_filter_velocity[axis].apply(data, N); - } - - // Apply general low-pass filter (IMU_GYRO_CUTOFF) - _lp_filter_velocity[axis].apply(data, N); - // save last filtered sample - angular_velocity_unscaled(axis) = data[N - 1]; - - - // angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) - float delta_velocity_filtered; - - for (int n = 0; n < N; n++) { - const float delta_velocity = (data[n] - _angular_velocity_prev(axis)); - delta_velocity_filtered = _lp_filter_acceleration[axis].apply(delta_velocity); - _angular_velocity_prev(axis) = data[n]; - } - - angular_acceleration_unscaled(axis) = delta_velocity_filtered / dt_s; + angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, N); + angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, data, N, dt_s); } - // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias - _angular_velocity = _calibration.Correct(angular_velocity_unscaled * sensor_fifo_data.scale) - _bias; - - // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation - _angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * sensor_fifo_data.scale; - // Publish if (!_sensor_fifo_sub.updated()) { - Publish(sensor_fifo_data.timestamp_sample); + CalibrateAndPublish(sensor_fifo_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled, + sensor_fifo_data.scale); } } } @@ -629,8 +660,9 @@ void VehicleAngularVelocity::Run() if (_reset_filters) { if (UpdateSampleRate()) { - _angular_velocity_prev = _angular_velocity; - ResetFilters(_angular_velocity, _angular_acceleration); + // non-FIFO sensor data is already scaled + _last_scale = 1.f; + ResetFilters(); } if (_reset_filters) { @@ -641,70 +673,39 @@ 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}; + Vector3f angular_velocity_unscaled; + Vector3f angular_acceleration_unscaled; + + float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z}; for (int axis = 0; axis < 3; axis++) { -#if !defined(CONSTRAINED_FLASH) + // copy sensor sample to float array for filtering + float data[1] {raw_data_array[axis]}; - // 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) { - for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) { - if (dnf[harmonic][axis].getNotchFreq() > 0.f) { - dnf[harmonic][axis].applyDF1(&angular_velocity(axis), 1); - - } else { - break; - } - } - } - - 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); - - // Apply general low-pass filter (IMU_GYRO_CUTOFF) - _lp_filter_velocity[axis].apply(&angular_velocity(axis), 1); - - // Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) - const float accel = (angular_velocity(axis) - _angular_velocity_prev(axis)) / dt_s; - _angular_acceleration(axis) = _lp_filter_acceleration[axis].apply(accel); - _angular_velocity_prev(axis) = angular_velocity(axis); + // save last filtered sample + angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, 1); + angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, data, 1, dt_s); } - _angular_velocity = angular_velocity; - // Publish if (!_sensor_sub.updated()) { - Publish(sensor_data.timestamp_sample); + CalibrateAndPublish(sensor_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled); } } } } -void VehicleAngularVelocity::Publish(const hrt_abstime ×tamp_sample) +void VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sample, + const Vector3f &angular_velocity_unscaled, const Vector3f &angular_acceleration_unscaled, float scale) { + // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias + _angular_velocity = _calibration.Correct(angular_velocity_unscaled * scale) - _bias; + + // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation + _angular_acceleration = _calibration.rotation() * angular_acceleration_unscaled * scale; + if (timestamp_sample >= _last_publish + _publish_interval_min_us) { + // Publish vehicle_angular_acceleration vehicle_angular_acceleration_s v_angular_acceleration; v_angular_acceleration.timestamp_sample = timestamp_sample; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 2b73ceaedb..9d71d5b9b3 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -75,11 +75,17 @@ public: private: void Run() override; + void CalibrateAndPublish(const hrt_abstime ×tamp_sample, const matrix::Vector3f &angular_velocity, + const matrix::Vector3f &angular_acceleration, float scale = 1.f); + + float FilterAngularVelocity(int axis, float data[], int N); + float FilterAngularAcceleration(int axis, float data[], int N, float dt_s); + 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 ResetFilters(); void SensorBiasUpdate(bool force = false); bool SensorSelectionUpdate(bool force = false); void UpdateDynamicNotchEscRpm(bool force = false); @@ -88,6 +94,7 @@ private: // scaled appropriately for current FIFO mode matrix::Vector3f GetResetAngularVelocity() const; + matrix::Vector3f GetResetAngularAcceleration() const; static constexpr int MAX_SENSOR_COUNT = 4;