fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: properly handle filter reset on FIFO data scale changes

Browse Source 
For the sake of efficiency (at 8 kHz) all filtering is performed on the raw data before the calibration and rotation is applied to only the final output. As a result we have to be a bit more careful when switching between sensors or in cases where the gyro scale factor changes (eg icm42688p 20 bit data rescaled to fit in int16 output).
This commit is contained in:
Daniel Agar 2021-04-25 14:20:32 -04:00 committed by GitHub
parent ed0fa99198
commit 8478d1ea37
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 146 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@ -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 &timestamp_sample, float x, float y, float z)
{
// Apply rotation (before scaling)

2
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@ -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 &timestamp_sample, float x, float y, float z);

14
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -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 &timestamp_sample, float x, float y, float z)
{
// Apply rotation (before scaling)

2
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -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 &timestamp_sample, float x, float y, float z);

197
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -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 &timestamp_sample,
void VehicleAngularVelocity::CalibrateAndPublish(bool publish, const hrt_abstime &timestamp_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 &timestamp_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;

18
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -75,26 +75,26 @@ public:
private:
void Run() override;
void CalibrateAndPublish(const hrt_abstime &timestamp_sample, const matrix::Vector3f &angular_velocity,
void CalibrateAndPublish(bool publish, const hrt_abstime &timestamp_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 */