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

gyro filtering: apply IMU_GYRO_CUTOFF also to D-term

Browse Source 
Greatly improves the noise on the d-term on a Kopis 2. Apparently the order
of filtering vs differentiating matters.

Also disables the DGYRO filter by default, as the D-term now already has
a default filter of 30Hz applied.
This commit is contained in:
Beat Küng
2020-06-04 20:58:40 -04:00
committed by Daniel Agar
parent 2069576a6e
commit 32642b78bf
2 changed files with 14 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+11 -8
View File
@@ -282,19 +282,22 @@ void VehicleAngularVelocity::Run()
// correct for in-run bias errors
const Vector3f angular_velocity_raw = _corrections.Correct(val) - _bias;
// Differentiate angular velocity (after notch filter)
const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};
const Vector3f angular_acceleration_raw = (angular_velocity_notched - _angular_velocity_prev) / dt;
// Gyro filtering:
// - Apply general notch filter (IMU_GYRO_NF_FREQ)
// - Apply general low-pass filter (IMU_GYRO_CUTOFF)
// - Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF)
_angular_velocity_prev = angular_velocity_notched;
const Vector3f angular_velocity_notched{_notch_filter_velocity.apply(angular_velocity_raw)};
const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
const Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
_angular_velocity_prev = angular_velocity;
_angular_acceleration_prev = angular_acceleration_raw;
const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
CheckFilters();
// Filter: apply low-pass
const Vector3f angular_acceleration{_lp_filter_acceleration.apply(angular_acceleration_raw)};
const Vector3f angular_velocity{_lp_filter_velocity.apply(angular_velocity_notched)};
// publish once all new samples are processed
sensor_updated = _sensor_sub[_selected_sensor_sub_index].updated();