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

mathlib: merge LowPassFilter2p, LowPassFilter2pArray, and LowPassFilter2pVector3f

Browse Source 
- fix reset for direct form 2
 - push NAN checks out to filter users
This commit is contained in:
Daniel Agar
2021-05-31 10:26:45 -04:00
parent 2a792ca201
commit 5493d96d17
23 changed files with 210 additions and 483 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/angular_velocity_controller/AngularVelocityController.hpp
-1
View File
@@ -35,7 +35,6 @@
#include <AngularVelocityControl.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
src/modules/mc_rate_control/RateControl/RateControl.hpp
-1
View File
@@ -40,7 +40,6 @@
#pragma once
#include <matrix/matrix/math.hpp>
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/mixer/MultirotorMixer/MultirotorMixer.hpp>
#include <uORB/topics/rate_ctrl_status.h>
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+19 -16
View File
@@ -227,26 +227,29 @@ void VehicleAcceleration::Run()
sensor_accel_s sensor_data;
while (_sensor_sub.update(&sensor_data)) {
const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z};
// Apply calibration and filter
// - calibration offsets, scale factors, and thermal scale (if available)
// - estimated in run bias (if available)
// - biquad low-pass filter
const Vector3f accel_corrected = _calibration.Correct(Vector3f{sensor_data.x, sensor_data.y, sensor_data.z}) - _bias;
const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
if (math::isFinite(accel_raw)) {
// Apply calibration and filter
// - calibration offsets, scale factors, and thermal scale (if available)
// - estimated in run bias (if available)
// - biquad low-pass filter
const Vector3f accel_corrected = _calibration.Correct(accel_raw) - _bias;
const Vector3f accel_filtered = _lp_filter.apply(accel_corrected);
_acceleration_prev = accel_corrected;
_acceleration_prev = accel_corrected;
// publish once all new samples are processed
if (!_sensor_sub.updated()) {
// Publish vehicle_acceleration
vehicle_acceleration_s v_acceleration;
v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
accel_filtered.copyTo(v_acceleration.xyz);
v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(v_acceleration);
// publish once all new samples are processed
if (!_sensor_sub.updated()) {
// Publish vehicle_acceleration
vehicle_acceleration_s v_acceleration;
v_acceleration.timestamp_sample = sensor_data.timestamp_sample;
accel_filtered.copyTo(v_acceleration.xyz);
v_acceleration.timestamp = hrt_absolute_time();
_vehicle_acceleration_pub.publish(v_acceleration);
return;
return;
}
}
}
}
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+2 -2
View File
@@ -36,7 +36,7 @@
#include <lib/sensor_calibration/Accelerometer.hpp>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@@ -96,7 +96,7 @@ private:
static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
float _filter_sample_rate{NAN};
math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.f};
math::LowPassFilter2p<matrix::Vector3f> _lp_filter{kInitialRateHz, 30.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+32 -29
View File
@@ -617,7 +617,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
}
// Apply general low-pass filter (IMU_GYRO_CUTOFF)
_lp_filter_velocity[axis].apply(data, N);
_lp_filter_velocity[axis].applyArray(data, N);
// return last filtered sample
return data[N - 1];
@@ -709,40 +709,43 @@ void VehicleAngularVelocity::Run()
sensor_gyro_s sensor_data;
while (_sensor_sub.update(&sensor_data)) {
if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {
_timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz;
}
if (PX4_ISFINITE(sensor_data.x) && PX4_ISFINITE(sensor_data.y) && PX4_ISFINITE(sensor_data.z)) {
const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), 0.00002f, 0.02f);
_timestamp_sample_last = sensor_data.timestamp_sample;
if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {
_timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz;
}
if (_reset_filters) {
ResetFilters();
const float dt_s = math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), 0.00002f, 0.02f);
_timestamp_sample_last = sensor_data.timestamp_sample;
if (_reset_filters) {
continue; // not safe to run until filters configured
ResetFilters();
if (_reset_filters) {
continue; // not safe to run until filters configured
}
}
UpdateDynamicNotchEscRpm();
UpdateDynamicNotchFFT();
Vector3f angular_velocity;
Vector3f angular_acceleration;
float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
for (int axis = 0; axis < 3; axis++) {
// copy sensor sample to float array for filtering
float data[1] {raw_data_array[axis]};
// save last filtered sample
angular_velocity(axis) = FilterAngularVelocity(axis, data);
angular_acceleration(axis) = FilterAngularAcceleration(axis, dt_s, data);
}
// Publish
CalibrateAndPublish(!_sensor_sub.updated(), sensor_data.timestamp_sample, angular_velocity, angular_acceleration);
}
UpdateDynamicNotchEscRpm();
UpdateDynamicNotchFFT();
Vector3f angular_velocity;
Vector3f angular_acceleration;
float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z};
for (int axis = 0; axis < 3; axis++) {
// copy sensor sample to float array for filtering
float data[1] {raw_data_array[axis]};
// save last filtered sample
angular_velocity(axis) = FilterAngularVelocity(axis, data);
angular_acceleration(axis) = FilterAngularAcceleration(axis, dt_s, data);
}
// Publish
CalibrateAndPublish(!_sensor_sub.updated(), sensor_data.timestamp_sample, angular_velocity, angular_acceleration);
}
}
}
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+2 -3
View File
@@ -38,7 +38,6 @@
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/NotchFilter.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
@@ -134,7 +133,7 @@ private:
static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
// angular velocity filters
math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::LowPassFilter2p<float> _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::NotchFilter<float> _notch_filter_velocity[3] {};
#if !defined(CONSTRAINED_FLASH)
@@ -168,7 +167,7 @@ private:
#endif // !CONSTRAINED_FLASH
// angular acceleration filter
math::LowPassFilter2p _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::LowPassFilter2p<float> _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
uint32_t _selected_sensor_device_id{0};