AC_PID: added feed-forward to AC_PID

libraries/AC_PID/AC_HELI_PID.cpp

@@ -23,7 +23,7 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
     // @Param: VFF
     // @DisplayName: Velocity FF FeedForward Gain
     // @Description: Velocity FF Gain which produces an output value that is proportional to the demanded input
-    AP_GROUPINFO("VFF",    4, AC_HELI_PID, _vff, 0),
+    AP_GROUPINFO("VFF",    4, AC_HELI_PID, _ff, 0),
 
     // @Param: IMAX
     // @DisplayName: PID Integral Maximum
@@ -48,19 +48,13 @@ 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_vff) :
+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)
 {
-    _vff = initial_vff;
+    _ff = initial_ff;
     _last_requested_rate = 0;
 }
 
-float AC_HELI_PID::get_vff(float requested_rate)
-{
-    _pid_info.FF = (float)requested_rate * _vff;
-    return _pid_info.FF;
-}
-
 // This is an integrator which tends to decay to zero naturally
 // if the error is zero.
 

libraries/AC_PID/AC_HELI_PID.h

@@ -19,9 +19,6 @@ public:
     /// Constructor for 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);
 
-    /// get_vff - return Velocity FeedForward Term 
-    float       get_vff(float requested_rate);
-
     /// get_leaky_i - replacement for get_i but output is leaded at leak_rate
     float       get_leaky_i(float leak_rate);
 

libraries/AC_PID/AC_PID.cpp

@@ -34,6 +34,11 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
     // @Units: Hz
     AP_GROUPINFO("FILT", 6, AC_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT),
 
+    // @Param: FF
+    // @DisplayName: FF FeedForward Gain
+    // @Description: FF Gain which produces an output value that is proportional to the demanded input
+    AP_GROUPINFO("FF",   7, AC_PID, _ff, 0),
+    
     AP_GROUPEND
 };
 
@@ -153,6 +158,13 @@ float AC_PID::get_d()
     return _pid_info.D;
 }
 
+float AC_PID::get_ff(float requested_rate)
+{
+    _pid_info.FF = (float)requested_rate * _ff;
+    return _pid_info.FF;
+}
+
+
 float AC_PID::get_pi()
 {
     return get_p() + get_i();

libraries/AC_PID/AC_PID.h

@@ -39,7 +39,8 @@ public:
     float       get_p();
     float       get_i();
     float       get_d();
-
+    float       get_ff(float requested_rate);
+    
     // reset_I - reset the integrator
     void        reset_I();
 
@@ -62,7 +63,7 @@ public:
     AP_Float   &filt_hz() { return _filt_hz; }
     float       imax() const { return _imax.get(); }
     float       get_filt_alpha() const;
-    float       ff() const { return _vff.get(); }
+    float       ff() const { return _ff.get(); }
 
     // set accessors
     void        kP(const float v) { _kp.set(v); }
@@ -70,7 +71,7 @@ public:
     void        kD(const float v) { _kd.set(v); }
     void        imax(const float v) { _imax.set(fabsf(v)); }
     void        filt_hz(const float v);
-    void        ff(const float v) { _vff.set(v); }
+    void        ff(const float v) { _ff.set(v); }
 
     float       get_integrator() const { return _integrator; }
     void        set_integrator(float i) { _integrator = i; }
@@ -91,7 +92,7 @@ protected:
     AP_Float        _kd;
     AP_Float        _imax;
     AP_Float        _filt_hz;                   // PID Input filter frequency in Hz
-    AP_Float        _vff; // only used by heli
+    AP_Float        _ff;
 
     // flags
     struct ac_pid_flags {