Tracker: support for upgrade to PID object

AntennaTracker/Parameters.h

@@ -170,8 +170,8 @@ public:
     AC_PID         pidYaw2Srv;
 
     Parameters() :
-        pidPitch2Srv(0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f),
+        pidPitch2Srv(0.2, 0.0f, 0.05f, 0.02f, 4000.0f, 0.0f, 0.0f, 0.0f, 0.1f),
-        pidYaw2Srv  (0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f)
+        pidYaw2Srv  (0.2, 0.0f, 0.05f, 0.02f, 4000.0f, 0.0f, 0.0f, 0.0f, 0.1f)
         {}
 };
 

AntennaTracker/control_auto.cpp

@@ -57,9 +57,9 @@ void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
     nav_status.angle_error_yaw = bf_yaw_err;
 
     // set actual and desired for logging, note we are using angles not rates
-    g.pidPitch2Srv.set_desired_rate(pitch * 0.01);
+    g.pidPitch2Srv.set_target_rate(pitch * 0.01);
     g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01);
-    g.pidYaw2Srv.set_desired_rate(yaw * 0.01);
+    g.pidYaw2Srv.set_target_rate(yaw * 0.01);
     g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01);
 }
 

AntennaTracker/servos.cpp

@@ -74,8 +74,7 @@ void Tracker::update_pitch_position_servo()
     // PITCH2SRV_IMAX   4000.000000
 
     // calculate new servo position
-    g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
+    int32_t new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch);
-    int32_t new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.get_pid();
 
     // position limit pitch servo
     if (new_servo_out <= pitch_min_cd) {
@@ -126,8 +125,7 @@ void Tracker::update_pitch_onoff_servo(float pitch)
 */
 void Tracker::update_pitch_cr_servo(float pitch)
 {
-    g.pidPitch2Srv.set_input_filter_all(nav_status.angle_error_pitch);
+    const float pitch_out = constrain_float(g.pidPitch2Srv.update_error(nav_status.angle_error_pitch), -(-g.pitch_min+g.pitch_max) * 100/2, (-g.pitch_min+g.pitch_max) * 100/2);
-    const float pitch_out = constrain_float(g.pidPitch2Srv.get_pid(), -(-g.pitch_min+g.pitch_max) * 100/2, (-g.pitch_min+g.pitch_max) * 100/2);
     SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_pitch, pitch_out);
 }
 
@@ -189,8 +187,7 @@ void Tracker::update_yaw_position_servo()
       right direction
      */
 
-    g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
+    float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw);
-    float servo_change = g.pidYaw2Srv.get_pid();
     servo_change = constrain_float(servo_change, -18000, 18000);
     float new_servo_out = constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_yaw) + servo_change, -18000, 18000);
 
@@ -241,7 +238,6 @@ void Tracker::update_yaw_onoff_servo(float yaw)
  */
 void Tracker::update_yaw_cr_servo(float yaw)
 {
-    g.pidYaw2Srv.set_input_filter_all(nav_status.angle_error_yaw);
+    const float yaw_out = constrain_float(-g.pidYaw2Srv.update_error(nav_status.angle_error_yaw), -g.yaw_range * 100/2, g.yaw_range * 100/2);
-    const float yaw_out = constrain_float(-g.pidYaw2Srv.get_pid(), -g.yaw_range * 100/2, g.yaw_range * 100/2);
     SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, yaw_out);
 }