diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 6eb7031053..497381e1e8 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -93,6 +93,22 @@ void PX4Accelerometer::set_device_type(uint8_t devtype) _device_id = device_id.devid; } +void PX4Accelerometer::set_scale(float scale) +{ + if (fabsf(scale - _scale) > FLT_EPSILON) { + // rescale last sample on scale change + float rescale = _scale / scale; + + for (auto &s : _last_sample) { + s = roundf(s * rescale); + } + + _scale = scale; + + UpdateClipLimit(); + } +} + void PX4Accelerometer::update(const hrt_abstime ×tamp_sample, float x, float y, float z) { // Apply rotation (before scaling) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 502c3a9ea1..a08ce14768 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -55,7 +55,7 @@ public: void set_error_count(uint32_t error_count) { _error_count = error_count; } void increase_error_count() { _error_count++; } void set_range(float range) { _range = range; UpdateClipLimit(); } - void set_scale(float scale) { _scale = scale; UpdateClipLimit(); } + void set_scale(float scale); void set_temperature(float temperature) { _temperature = temperature; } void update(const hrt_abstime ×tamp_sample, float x, float y, float z); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index a5002e70ca..d816893e26 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -80,6 +80,20 @@ void PX4Gyroscope::set_device_type(uint8_t devtype) _device_id = device_id.devid; } +void PX4Gyroscope::set_scale(float scale) +{ + if (fabsf(scale - _scale) > FLT_EPSILON) { + // rescale last sample on scale change + float rescale = _scale / scale; + + for (auto &s : _last_sample) { + s = roundf(s * rescale); + } + + _scale = scale; + } +} + void PX4Gyroscope::update(const hrt_abstime ×tamp_sample, float x, float y, float z) { // Apply rotation (before scaling) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index ff97374b84..ef4c3c513b 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -55,7 +55,7 @@ public: void set_error_count(uint32_t error_count) { _error_count = error_count; } void increase_error_count() { _error_count++; } void set_range(float range) { _range = range; } - void set_scale(float scale) { _scale = scale; } + void set_scale(float scale); void set_temperature(float temperature) { _temperature = temperature; } void update(const hrt_abstime ×tamp_sample, float x, float y, float z); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index c1822bcb66..cfbcd39bfb 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -106,10 +106,12 @@ bool VehicleAngularVelocity::UpdateSampleRate() } // calculate sensor update rate - if (PX4_ISFINITE(sample_rate_hz) && PX4_ISFINITE(publish_rate_hz)) { + if ((sample_rate_hz > 0) && PX4_ISFINITE(sample_rate_hz) && (publish_rate_hz > 0) && PX4_ISFINITE(publish_rate_hz)) { // check if sample rate error is greater than 1% - if ((fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f) { - PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz); + if ((_filter_sample_rate_hz <= FLT_EPSILON) || !PX4_ISFINITE(_filter_sample_rate_hz) + || (fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f) { + + PX4_DEBUG("updating sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz); _reset_filters = true; _filter_sample_rate_hz = sample_rate_hz; @@ -137,46 +139,46 @@ bool VehicleAngularVelocity::UpdateSampleRate() _publish_interval_min_us = 0; } } - - if (_filter_sample_rate_hz > 0.f) { - return true; - } } - return false; + return PX4_ISFINITE(_filter_sample_rate_hz) && (_filter_sample_rate_hz > 0); } -void VehicleAngularVelocity::ResetFilters() +void VehicleAngularVelocity::ResetFilters(float new_scale) { - const Vector3f angular_velocity{GetResetAngularVelocity()}; - const Vector3f angular_acceleration{GetResetAngularAcceleration()}; + if ((_filter_sample_rate_hz > 0) && PX4_ISFINITE(_filter_sample_rate_hz)) { - 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()); - _lp_filter_velocity[axis].reset(angular_velocity(axis)); + const Vector3f angular_velocity{GetResetAngularVelocity(new_scale)}; + const Vector3f angular_acceleration{GetResetAngularAcceleration(new_scale)}; - // 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(axis)); + 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()); + _lp_filter_velocity[axis].reset(angular_velocity(axis)); - // 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)); + // 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(axis)); + + // 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(new_scale, true); + UpdateDynamicNotchFFT(new_scale, true); + + _angular_velocity_prev = angular_velocity; + _last_scale = new_scale; + + _reset_filters = false; + perf_count(_filter_reset_perf); } - - // 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); } void VehicleAngularVelocity::SensorBiasUpdate(bool force) @@ -224,6 +226,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) _selected_sensor_device_id = sensor_selection.gyro_device_id; _calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id); + _timestamp_sample_last = 0; + _filter_sample_rate_hz = NAN; _reset_filters = true; _bias.zero(); _fifo_available = true; @@ -231,6 +235,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) perf_count(_selection_changed_perf); + PX4_DEBUG("selecting gyro FIFO %d %d", i, _selected_sensor_device_id); + return true; } } @@ -249,7 +255,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) _calibration.set_device_id(sensor_gyro_sub.get().device_id); _selected_sensor_device_id = sensor_selection.gyro_device_id; - // clear bias and corrections + _timestamp_sample_last = 0; + _filter_sample_rate_hz = NAN; _reset_filters = true; _bias.zero(); _fifo_available = false; @@ -257,6 +264,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) perf_count(_selection_changed_perf); + PX4_DEBUG("selecting gyro %d %d", i, _selected_sensor_device_id); + return true; } } @@ -341,29 +350,37 @@ void VehicleAngularVelocity::ParametersUpdate(bool force) } } -Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const +Vector3f VehicleAngularVelocity::GetResetAngularVelocity(float new_scale) const { - if ((_last_publish != 0) && (_last_scale > 0.f) - && PX4_ISFINITE(_angular_velocity(0)) - && PX4_ISFINITE(_angular_velocity(1)) - && PX4_ISFINITE(_angular_velocity(2))) { + if ((_last_publish != 0) && (new_scale > 0.f)) { // 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; + Vector3f angular_velocity{_calibration.Uncorrect(_angular_velocity + _bias) / new_scale}; + + if (PX4_ISFINITE(angular_velocity(0)) + && PX4_ISFINITE(angular_velocity(1)) + && PX4_ISFINITE(angular_velocity(2))) { + + return angular_velocity; + } } return Vector3f{0.f, 0.f, 0.f}; } -Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const +Vector3f VehicleAngularVelocity::GetResetAngularAcceleration(float new_scale) 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 + if ((_last_publish != 0) && (new_scale > 0.f)) { + // angular acceleration filtering is performed on unscaled angular velocity 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; + Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration / new_scale}; + + if (PX4_ISFINITE(angular_acceleration(0)) + && PX4_ISFINITE(angular_acceleration(1)) + && PX4_ISFINITE(angular_acceleration(2))) { + + return angular_acceleration; + } } return Vector3f{0.f, 0.f, 0.f}; @@ -401,7 +418,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT() #endif // !CONSTRAINED_FLASH } -void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) +void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(float new_scale, bool force) { #if !defined(CONSTRAINED_FLASH) const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm; @@ -434,8 +451,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) } // only reset if there's sufficient change (> 1%) - if (change_percent > 0.01f) { - const Vector3f reset_angular_velocity{GetResetAngularVelocity()}; + if (force || (change_percent > 0.01f)) { + const Vector3f reset_angular_velocity{GetResetAngularVelocity(new_scale)}; for (int axis = 0; axis < 3; axis++) { auto &dnf = _dynamic_notch_filter_esc_rpm[i][harmonic][axis]; @@ -464,7 +481,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force) #endif // !CONSTRAINED_FLASH } -void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) +void VehicleAngularVelocity::UpdateDynamicNotchFFT(float new_scale, bool force) { #if !defined(CONSTRAINED_FLASH) const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT; @@ -488,13 +505,13 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force) const float peak_diff_abs = fabsf(dnf.getNotchFreq() - peak_freq); const float change_percent = peak_diff_abs / peak_freq; - if (change_percent > 0.001f) { + if (force || (change_percent > 0.001f)) { // peak frequency changed by at least 0.1% dnf.setParameters(_filter_sample_rate_hz, peak_freq, sensor_gyro_fft.resolution_hz); // only reset if there's sufficient change if (peak_diff_abs > sensor_gyro_fft.resolution_hz) { - dnf.reset(GetResetAngularVelocity()(axis)); + dnf.reset(GetResetAngularVelocity(new_scale)(axis)); } perf_count(_dynamic_notch_filter_fft_update_perf); @@ -566,22 +583,18 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int return data[N - 1]; } -float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float data[], int N, float dt_s) +float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float dt_s, float data[], int N) { - if (N > 0) { - // angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) - float delta_velocity_filtered; + // angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) + float angular_acceleration_filtered = 0.f; - 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; + for (int n = 0; n < N; n++) { + const float angular_acceleration = (data[n] - _angular_velocity_prev(axis)) / dt_s; + angular_acceleration_filtered = _lp_filter_acceleration[axis].apply(angular_acceleration); + _angular_velocity_prev(axis) = data[n]; } - return 0.f; + return angular_acceleration_filtered; } void VehicleAngularVelocity::Run() @@ -589,12 +602,20 @@ void VehicleAngularVelocity::Run() // backup schedule ScheduleDelayed(10_ms); + ParametersUpdate(); + // update corrections first to set _selected_sensor const bool selection_updated = SensorSelectionUpdate(); _calibration.SensorCorrectionsUpdate(selection_updated); SensorBiasUpdate(selection_updated); - ParametersUpdate(); + + if (selection_updated || !PX4_ISFINITE(_filter_sample_rate_hz) || (_filter_sample_rate_hz <= FLT_EPSILON)) { + if (!UpdateSampleRate()) { + // sensor sample rate required to run + return; + } + } if (_fifo_available) { // process all outstanding fifo messages @@ -604,13 +625,9 @@ void VehicleAngularVelocity::Run() const float dt_s = sensor_fifo_data.dt * 1e-6f; _timestamp_sample_last = sensor_fifo_data.timestamp_sample; + // in FIFO mode the unscaled raw data is filtered, reset filters on any scale change 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; - - ResetFilters(); - } + ResetFilters(sensor_fifo_data.scale); if (_reset_filters) { continue; // not safe to run until filters configured @@ -639,14 +656,12 @@ void VehicleAngularVelocity::Run() // save last filtered sample angular_velocity_unscaled(axis) = FilterAngularVelocity(axis, data, N); - angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, data, N, dt_s); + angular_acceleration_unscaled(axis) = FilterAngularAcceleration(axis, dt_s, data, N); } // Publish - if (!_sensor_fifo_sub.updated()) { - CalibrateAndPublish(sensor_fifo_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled, - sensor_fifo_data.scale); - } + CalibrateAndPublish(!_sensor_fifo_sub.updated(), sensor_fifo_data.timestamp_sample, angular_velocity_unscaled, + angular_acceleration_unscaled, sensor_fifo_data.scale); } } @@ -655,15 +670,15 @@ void VehicleAngularVelocity::Run() sensor_gyro_s sensor_data; while (_sensor_sub.update(&sensor_data)) { + if (_timestamp_sample_last == 0) { + _timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz; + } + const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) / 1e6f), 0.0002f, 0.02f); _timestamp_sample_last = sensor_data.timestamp_sample; if (_reset_filters) { - if (UpdateSampleRate()) { - // non-FIFO sensor data is already scaled - _last_scale = 1.f; - ResetFilters(); - } + ResetFilters(); if (_reset_filters) { continue; // not safe to run until filters configured @@ -673,8 +688,8 @@ void VehicleAngularVelocity::Run() UpdateDynamicNotchEscRpm(); UpdateDynamicNotchFFT(); - Vector3f angular_velocity_unscaled; - Vector3f angular_acceleration_unscaled; + Vector3f angular_velocity; + Vector3f angular_acceleration; float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z}; @@ -683,19 +698,17 @@ void VehicleAngularVelocity::Run() float data[1] {raw_data_array[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(axis) = FilterAngularVelocity(axis, data); + angular_acceleration(axis) = FilterAngularAcceleration(axis, dt_s, data); } // Publish - if (!_sensor_sub.updated()) { - CalibrateAndPublish(sensor_data.timestamp_sample, angular_velocity_unscaled, angular_acceleration_unscaled); - } + CalibrateAndPublish(!_sensor_sub.updated(), sensor_data.timestamp_sample, angular_velocity, angular_acceleration); } } } -void VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sample, +void VehicleAngularVelocity::CalibrateAndPublish(bool publish, 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 @@ -704,7 +717,7 @@ void VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sa // 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) { + if (publish && (timestamp_sample >= _last_publish + _publish_interval_min_us)) { // Publish vehicle_angular_acceleration vehicle_angular_acceleration_s v_angular_acceleration; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 9d71d5b9b3..bce91d9771 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -75,26 +75,26 @@ public: private: void Run() override; - void CalibrateAndPublish(const hrt_abstime ×tamp_sample, const matrix::Vector3f &angular_velocity, + void CalibrateAndPublish(bool publish, 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); + float FilterAngularVelocity(int axis, float data[], int N = 1); + float FilterAngularAcceleration(int axis, float dt_s, float data[], int N = 1); void DisableDynamicNotchEscRpm(); void DisableDynamicNotchFFT(); void ParametersUpdate(bool force = false); - void ResetFilters(); + void ResetFilters(float new_scale = 1.f); void SensorBiasUpdate(bool force = false); bool SensorSelectionUpdate(bool force = false); - void UpdateDynamicNotchEscRpm(bool force = false); - void UpdateDynamicNotchFFT(bool force = false); + void UpdateDynamicNotchEscRpm(float new_scale = 1.f, bool force = false); + void UpdateDynamicNotchFFT(float new_scale = 1.f, bool force = false); bool UpdateSampleRate(); // scaled appropriately for current FIFO mode - matrix::Vector3f GetResetAngularVelocity() const; - matrix::Vector3f GetResetAngularAcceleration() const; + matrix::Vector3f GetResetAngularVelocity(float new_scale = 1.f) const; + matrix::Vector3f GetResetAngularAcceleration(float new_scale = 1.f) const; static constexpr int MAX_SENSOR_COUNT = 4; @@ -127,7 +127,7 @@ private: hrt_abstime _publish_interval_min_us{0}; hrt_abstime _last_publish{0}; - float _filter_sample_rate_hz{0.f}; + float _filter_sample_rate_hz{NAN}; static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */