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

sensors: move gyro filtering to sensors/vehicle_angular_velocity

Browse Source 
- gyro filtering (low-pass and notch) only performed on primary gyro in `sensors/vehicle_angular_velocity` instead of every gyro in `PX4Gyroscope`
 - sample rate is calculated from actual updates (the fixed value was slightly wrong in many cases, and very wrong in a few)
 - In the FIFO case the array is now averaged and published in `sensor_gyro` for filtering downstream. I'll update this in the future to use the full FIFO array (if available), but right now it should be fine.
This commit is contained in:
Daniel Agar
2020-01-27 10:05:33 -05:00
committed by GitHub
parent 0d2e5f1c50
commit 24f0c2d72a
22 changed files with 355 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+20
View File
@@ -36,6 +36,9 @@
#include <lib/conversion/rotation.h>
#include <lib/mathlib/math/Limits.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <lib/mathlib/math/filter/NotchFilter.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@@ -65,6 +68,7 @@ public:
private:
void Run() override;
void CheckFilters(const matrix::Vector3f &rates);
void ParametersUpdate(bool force = false);
void SensorBiasUpdate(bool force = false);
void SensorCorrectionsUpdate(bool force = false);
@@ -73,6 +77,11 @@ private:
static constexpr int MAX_SENSOR_COUNT = 3;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
@@ -93,12 +102,23 @@ private:
{this, ORB_ID(sensor_gyro), 2}
};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
matrix::Dcmf _board_rotation;
matrix::Vector3f _bias{0.f, 0.f, 0.f};
matrix::Vector3f _offset{0.f, 0.f, 0.f};
matrix::Vector3f _scale{1.f, 1.f, 1.f};
hrt_abstime _last_publish{0};
hrt_abstime _filter_check_last{0};
static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
math::LowPassFilter2pVector3f _lowpass_filter{kInitialRateHz, 30};
math::NotchFilter<matrix::Vector3f> _notch_filter{};
float _filter_sample_rate{kInitialRateHz};
int _sample_rate_incorrect_count{0};
uint32_t _selected_sensor_device_id{0};
uint8_t _selected_sensor_sub_index{0};
int8_t _corrections_selected_instance{-1};