From 58257e3858c6d5234a3ff9775d380a34935bcadf Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 11 Aug 2014 21:57:50 +0900 Subject: [PATCH] AC_AttControl: set rate D term filter from ins filter --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 9350d02b4d..d4c795f809 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -73,10 +73,19 @@ void AC_AttitudeControl::set_dt(float delta_sec) { _dt = delta_sec; + // get filter from ahrs + const AP_InertialSensor &ins = _ahrs.get_ins(); + float ins_filter = (float)ins.get_filter(); + + // sanity check filter + if (ins_filter <= 0.0f) { + ins_filter = AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER; + } + // set attitude controller's D term filters - _pid_rate_roll.set_d_lpf_alpha(AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER, _dt); - _pid_rate_pitch.set_d_lpf_alpha(AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER, _dt); - _pid_rate_yaw.set_d_lpf_alpha(AC_ATTITUDE_RATE_Y_PID_DTERM_FILTER, _dt); + _pid_rate_roll.set_d_lpf_alpha(ins_filter, _dt); + _pid_rate_pitch.set_d_lpf_alpha(ins_filter, _dt); + _pid_rate_yaw.set_d_lpf_alpha(ins_filter/2.0f, _dt); // half } // relax_bf_rate_controller - ensure body-frame rate controller has zero errors to relax rate controller output