AP_Winch: support for upgrade to PID object

libraries/AP_Winch/AP_Winch.h

@@ -82,7 +82,7 @@ private:
         AP_Int8     type;               // winch type
         AP_Float    rate_max;           // deploy or retract rate maximum (in m/s).
         AP_Float    pos_p;              // position error P gain
-        AC_PID      rate_pid = AC_PID(AP_WINCH_RATE_P, AP_WINCH_RATE_I, AP_WINCH_RATE_D, AP_WINCH_RATE_IMAX, AP_WINCH_RATE_FILT, AP_WINCH_RATE_DT);           // rate control PID
+        AC_PID      rate_pid = AC_PID(AP_WINCH_RATE_P, AP_WINCH_RATE_I, AP_WINCH_RATE_D, 0.0f, AP_WINCH_RATE_IMAX, 0.0f, AP_WINCH_RATE_FILT, 0.0f, AP_WINCH_RATE_DT);           // rate control PID
         winch_state state;              // state of winch control (using target position or target rate)
         float       length_curr;        // current length of the line (in meters) that has been deployed
         float       length_desired;     // target desired length (in meters)

libraries/AP_Winch/AP_Winch_Servo.cpp

@@ -58,22 +58,6 @@ void AP_Winch_Servo::update()
         rate_desired = config.rate_desired;
     }
 
-    // calculate rate error and pass to pid controller
-    float rate_error = rate_desired - rate;
-    config.rate_pid.set_input_filter_all(rate_error);
-
-    // get p
-    float p = config.rate_pid.get_p();
-
-    // get i unless winch hit limit on last iteration
-    float i = config.rate_pid.get_integrator();
-    if (((is_negative(rate_error) && !limit_low) || (is_positive(rate_error) && !limit_high))) {
-        i = config.rate_pid.get_i();
-    }
-
-    // get d
-    float d = config.rate_pid.get_d();
-
     // calculate base output
     float base = 0.0f;
     if (is_positive(config.rate_max)) {
@@ -81,7 +65,7 @@ void AP_Winch_Servo::update()
     }
 
     // constrain and set limit flags
-    float output = base + p + i + d;
+    float output = base + config.rate_pid.update_all(rate_desired, rate, (limit_low || limit_high));
     limit_low = (output <= -1.0f);
     limit_high = (output >= 1.0f);
     output = constrain_float(output, -1.0f, 1.0f);