aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttControl: set rate D term filter from ins filter

Browse Source
This commit is contained in:
Randy Mackay 2014-08-11 21:57:50 +09:00
parent 51f40fe3d6
commit 58257e3858
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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