From 43834e3fafa09ba38fe6f995cadcf191e666250a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 00:20:17 +0200 Subject: [PATCH] Att Q estimator: Lowpass output for all three rates with adjusted lowpass frequencies --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 926e2dc592..a9ffbe0c42 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -160,6 +160,7 @@ private: /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; math::LowPassFilter2p _lp_pitch_rate; + math::LowPassFilter2p _lp_yaw_rate; hrt_abstime _vel_prev_t = 0; @@ -187,8 +188,9 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _voter_gyro(3), _voter_accel(3), _voter_mag(3), - _lp_roll_rate(250.0f, 20.0f), - _lp_pitch_rate(250.0f, 20.0f) + _lp_roll_rate(250.0f, 18.0f), + _lp_pitch_rate(250.0f, 18.0f), + _lp_yaw_rate(250, 10.0f) { _voter_mag.set_timeout(200000); @@ -434,7 +436,7 @@ void AttitudeEstimatorQ::task_main() */ att.rollspeed = _lp_roll_rate.apply(_rates(0)); att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); - att.yawspeed = _rates(2); + att.yawspeed = _lp_yaw_rate.apply(_rates(2)); for (int i = 0; i < 3; i++) { att.g_comp[i] = _accel(i) - _pos_acc(i);