From fff81a2163ed1ecf93394dd542daa1cf6cecf349 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 23 May 2019 22:14:01 +0100 Subject: [PATCH] AP_InertialSensor: Make sure the LPF output is used by the notch input. --- libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 73f443f670..c69a046094 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -208,7 +208,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro); // apply the notch filter if (_gyro_notch_enabled()) { - _imu._gyro_filtered[instance] = _imu._gyro_notch_filter[instance].apply(gyro); + _imu._gyro_filtered[instance] = _imu._gyro_notch_filter[instance].apply(_imu._gyro_filtered[instance]); } if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) { _imu._gyro_filter[instance].reset();