From 4e8155f4e5576222dfc67d7fdd35e01e91eef417 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 3 Jan 2018 11:41:37 +0900 Subject: [PATCH] AC_PID: allow non-zero ff gain default --- libraries/AC_PID/AC_HELI_PID.cpp | 3 +-- libraries/AC_PID/AC_PID.cpp | 6 ++++-- libraries/AC_PID/AC_PID.h | 4 ++-- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp index a0a66bcd43..0480703075 100644 --- a/libraries/AC_PID/AC_HELI_PID.cpp +++ b/libraries/AC_PID/AC_HELI_PID.cpp @@ -49,9 +49,8 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = { /// Constructor for PID AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff) : - AC_PID(initial_p, initial_i, initial_d, initial_imax, initial_filt_hz, dt) + AC_PID(initial_p, initial_i, initial_d, initial_imax, initial_filt_hz, dt, initial_ff) { - _ff = initial_ff; _last_requested_rate = 0; } diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index a800af34ac..a3a564bd91 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -43,7 +43,7 @@ const AP_Param::GroupInfo AC_PID::var_info[] = { }; // Constructor -AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt) : +AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff) : _dt(dt), _integrator(0.0f), _input(0.0f), @@ -57,6 +57,7 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ _kd = initial_d; _imax = fabsf(initial_imax); filt_hz(initial_filt_hz); + _ff = initial_ff; // reset input filter to first value received _flags._reset_filter = true; @@ -201,7 +202,7 @@ void AC_PID::save_gains() } /// Overload the function call operator to permit easy initialisation -void AC_PID::operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt) +void AC_PID::operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt, float ffval) { _kp = p; _ki = i; @@ -209,6 +210,7 @@ void AC_PID::operator() (float p, float i, float d, float imaxval, float input_f _imax = fabsf(imaxval); _filt_hz = input_filt_hz; _dt = dt; + _ff = ffval; } // calc_filt_alpha - recalculate the input filter alpha diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index e6ba4539f1..a3cf75bc16 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -18,7 +18,7 @@ class AC_PID { public: // Constructor for PID - AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt); + AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff = 0); // set_dt - set time step in seconds void set_dt(float dt); @@ -54,7 +54,7 @@ public: void save_gains(); /// operator function call for easy initialisation - void operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt ); + void operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt, float ffval = 0); // get accessors AP_Float &kP() { return _kp; }