dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gyro filtering: apply IMU_GYRO_CUTOFF also to D-term 

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-03 11:52:13 +02:00 committed by Daniel Agar
parent 2069576a6e
commit 32642b78bf
2 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
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();

6
src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c
View File

@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF_BW, 20.0f);
*
* The cutoff frequency for the 2nd order butterworth filter on the primary gyro.
* This only affects the angular velocity sent to the controllers, not the estimators.
* Doesn't apply to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF.
* It applies also to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF.
*
* A value of 0 disables the filter.
*
@ -109,7 +109,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
* the time derivative of the measured angular velocity, also known as
* the D-term filter in the rate controller. The D-term uses the derivative of
* the rate and thus is the most susceptible to noise. Therefore, using
* a D-term filter allows to decrease the driver-level filtering, which
* a D-term filter allows to increase IMU_GYRO_CUTOFF, which
* leads to reduced control latency and permits to increase the P gains.
*
* A value of 0 disables the filter.
@ -120,4 +120,4 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
* @reboot_required true
* @group Sensors
*/
PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f);
PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 0.0f);