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

AC_AttControl: set roll, pitch, yaw rate control D term filters

Browse Source
This commit is contained in:
Randy Mackay 2014-05-27 22:44:29 +09:00
parent faf3415e5e
commit 0969e464fb
2 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -59,6 +59,16 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = {
// high level controllers
//
void AC_AttitudeControl::set_dt(float delta_sec)
{
_dt = delta_sec;
// 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);
}
// init_targets - resets target angles to current angles
void AC_AttitudeControl::init_targets()
{

5
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -37,6 +37,9 @@
#define AC_ATTITUDE_100HZ_DT 0.0100f // delta time in seconds for 100hz update rate
#define AC_ATTITUDE_400HZ_DT 0.0025f // delta time in seconds for 400hz update rate
#define AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER 20 // D-term filter rate cutoff frequency for Roll and Pitch rate controllers
#define AC_ATTITUDE_RATE_Y_PID_DTERM_FILTER 5 // D-term filter rate cutoff frequency for Yaw rate controller
class AC_AttitudeControl {
public:
AC_AttitudeControl( AP_AHRS &ahrs,
@ -70,7 +73,7 @@ public:
//
// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
void set_dt(float delta_sec) { _dt = delta_sec; }
void set_dt(float delta_sec);
// init_targets - resets target angles to current angles
void init_targets();