Tracker: Changed PID to AC_PID

AntennaTracker/Parameters.cpp

@@ -337,7 +337,7 @@ const AP_Param::Info Tracker::var_info[] = {
     // @Range: 0.001 0.1
     // @Increment: 0.001
     // @User: Standard
-	GGROUP(pidPitch2Srv,       "PITCH2SRV_", PID),
+	GGROUP(pidPitch2Srv,       "PITCH2SRV_", AC_PID),
 
     // @Param: YAW2SRV_P
     // @DisplayName: Yaw axis controller P gain
@@ -367,7 +367,7 @@ const AP_Param::Info Tracker::var_info[] = {
     // @Range: 0.001 0.1
     // @Increment: 0.001
     // @User: Standard
-	GGROUP(pidYaw2Srv,         "YAW2SRV_", PID),
+	GGROUP(pidYaw2Srv,         "YAW2SRV_", AC_PID),
 
     // @Param: CMD_TOTAL
     // @DisplayName: Number of loaded mission items

AntennaTracker/Parameters.h

@@ -63,8 +63,8 @@ public:
         k_param_scheduler,
         k_param_ins,
         k_param_sitl,
-        k_param_pidPitch2Srv,
-        k_param_pidYaw2Srv,
+        k_param_pidPitch_old,   // deprecated
+        k_param_pidYaw_old,     // deprecated
         k_param_gcs2,               // stream rates for uartD
         k_param_serial2_baud,       // deprecated
 
@@ -107,6 +107,8 @@ public:
         //
         k_param_channel_yaw = 200,
         k_param_channel_pitch,
+        k_param_pidPitch2Srv,
+        k_param_pidYaw2Srv,
 
         //
         // 220: Waypoint data
@@ -156,13 +158,13 @@ public:
 
     AP_Int32 log_bitmask;
 
-    // PID controllers
-    PID         pidPitch2Srv;
-    PID         pidYaw2Srv;
+    // AC_PID controllers
+    AC_PID         pidPitch2Srv;
+    AC_PID         pidYaw2Srv;
 
     Parameters() :
-        pidPitch2Srv(0.2, 0, 0.05f, 4000.0f),
-        pidYaw2Srv  (0.2, 0, 0.05f, 4000.0f)
+        pidPitch2Srv(0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f),
+        pidYaw2Srv  (0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f)
         {}
 };
 

AntennaTracker/Tracker.h

@@ -46,7 +46,7 @@
 #include <AP_SerialManager/AP_SerialManager.h>   // Serial manager library
 #include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
 #include <DataFlash/DataFlash.h>
-#include <PID/PID.h>
+#include <AC_PID/AC_PID.h>
 #include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
 #include <AP_NavEKF/AP_NavEKF.h>
 #include <AP_NavEKF2/AP_NavEKF2.h>

AntennaTracker/make.inc

@@ -15,7 +15,7 @@ LIBRARIES += GCS_MAVLink
 LIBRARIES += AP_SerialManager
 LIBRARIES += AP_Declination
 LIBRARIES += DataFlash
-LIBRARIES += PID
+LIBRARIES += AC_PID
 LIBRARIES += AP_Scheduler
 LIBRARIES += AP_NavEKF
 LIBRARIES += AP_NavEKF2

AntennaTracker/servos.cpp

@@ -78,11 +78,8 @@ void Tracker::update_pitch_position_servo(float pitch)
     // PITCH2SRV_IMAX   4000.000000
 
     // calculate new servo position
-    int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid(angle_err);
-
-    // initialise limit flags
-    servo_limit.pitch_lower = false;
-    servo_limit.pitch_upper = false;
+    g.pidPitch2Srv.set_input_filter_all(angle_err);
+    int32_t new_servo_out = channel_pitch.get_servo_out() + g.pidPitch2Srv.get_pid();
 
     // rate limit pitch servo
     if (g.pitch_slew_time > 0.02f) {
@@ -92,11 +89,9 @@ void Tracker::update_pitch_position_servo(float pitch)
         }
         if (new_servo_out <= channel_pitch.get_servo_out() - max_change) {
             new_servo_out = channel_pitch.get_servo_out() - max_change;
-            servo_limit.pitch_lower = true;
         }
         if (new_servo_out >= channel_pitch.get_servo_out() + max_change) {
             new_servo_out = channel_pitch.get_servo_out() + max_change;
-            servo_limit.pitch_upper = true;
         }
     }
     channel_pitch.set_servo_out(new_servo_out);
@@ -104,11 +99,11 @@ void Tracker::update_pitch_position_servo(float pitch)
     // position limit pitch servo
     if (channel_pitch.get_servo_out() <= -pitch_limit_cd) {
         channel_pitch.set_servo_out(-pitch_limit_cd);
-        servo_limit.pitch_lower = true;
+        g.pidPitch2Srv.reset_I();
     }
     if (channel_pitch.get_servo_out() >= pitch_limit_cd) {
         channel_pitch.set_servo_out(pitch_limit_cd);
-        servo_limit.pitch_upper = true;
+        g.pidPitch2Srv.reset_I();
     }
 }
 
@@ -146,7 +141,8 @@ void Tracker::update_pitch_cr_servo(float pitch)
 {
     float ahrs_pitch = degrees(ahrs.pitch);
     float err_cd = (pitch - ahrs_pitch) * 100.0f;
-    channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid(err_cd));
+    g.pidPitch2Srv.set_input_filter_all(err_cd);
+    channel_pitch.set_servo_out(g.pidPitch2Srv.get_pid());
 }
 
 /**
@@ -256,17 +252,13 @@ void Tracker::update_yaw_position_servo(float yaw)
     }
 
     slew_dir = new_slew_dir;
-
-    // initialise limit flags
-    servo_limit.yaw_lower = false;
-    servo_limit.yaw_upper = false;
-
     int16_t new_servo_out;
     if (slew_dir != 0) {
         new_servo_out = slew_dir * 18000;
         g.pidYaw2Srv.reset_I();
     } else {
-        float servo_change = g.pidYaw2Srv.get_pid(angle_err);
+        g.pidYaw2Srv.set_input_filter_all(angle_err);
+        float servo_change = g.pidYaw2Srv.get_pid();
         servo_change = constrain_float(servo_change, -18000, 18000);
         new_servo_out = constrain_float(channel_yaw.get_servo_out() - servo_change, -18000, 18000);
     }
@@ -279,23 +271,21 @@ void Tracker::update_yaw_position_servo(float yaw)
         }
         if (new_servo_out <= channel_yaw.get_servo_out() - max_change) {
             new_servo_out = channel_yaw.get_servo_out() - max_change;
-            servo_limit.yaw_lower = true;
         }
         if (new_servo_out >= channel_yaw.get_servo_out() + max_change) {
             new_servo_out = channel_yaw.get_servo_out() + max_change;
-            servo_limit.yaw_upper = true;
         }
     }
     channel_yaw.set_servo_out(new_servo_out);
 
-    // position limit pitch servo
+    // position limit yaw servo
     if (channel_yaw.get_servo_out() <= -yaw_limit_cd) {
         channel_yaw.set_servo_out(-yaw_limit_cd);
-        servo_limit.yaw_lower = true;
+        g.pidYaw2Srv.reset_I();
     }
     if (channel_yaw.get_servo_out() >= yaw_limit_cd) {
         channel_yaw.set_servo_out(yaw_limit_cd);
-        servo_limit.yaw_upper = true;
+        g.pidYaw2Srv.reset_I();
     }
 }
 
@@ -335,5 +325,6 @@ void Tracker::update_yaw_cr_servo(float yaw)
     float yaw_cd = wrap_180_cd(yaw*100.0f);
     float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd);
 
-    channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid(err_cd));
+    g.pidYaw2Srv.set_input_filter_all(err_cd);
+    channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid());
 }

AntennaTracker/wscript

@@ -7,7 +7,7 @@ def build(bld):
         name=vehicle + '_libs',
         vehicle=vehicle,
         libraries=bld.ap_common_vehicle_libraries() + [
-            'PID',
+            'AC_PID',
         ],
         use='mavlink',
     )