From d61aa7a4ce846754cf20360820276567fbb604cf Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 27 Jun 2019 19:06:31 +0930 Subject: [PATCH] Tracker: support for upgrade to PID object --- AntennaTracker/Parameters.h | 4 ++-- AntennaTracker/control_auto.cpp | 4 ++-- AntennaTracker/servos.cpp | 12 ++++-------- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 1de33ecaea..ef68f3b092 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -170,8 +170,8 @@ public: AC_PID pidYaw2Srv; Parameters() : - pidPitch2Srv(0.2, 0, 0.05f, 4000.0f, 0.1, 0.02f), - pidYaw2Srv (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.0f, 0.05f, 0.02f, 4000.0f, 0.0f, 0.0f, 0.0f, 0.1f) {} }; diff --git a/AntennaTracker/control_auto.cpp b/AntennaTracker/control_auto.cpp index a6e8c8c782..b6aac57159 100644 --- a/AntennaTracker/control_auto.cpp +++ b/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); } diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index 89bf3aa3b7..a845111f63 100644 --- a/AntennaTracker/servos.cpp +++ b/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.get_pid(); + int32_t new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch); // 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.get_pid(), -(-g.pitch_min+g.pitch_max) * 100/2, (-g.pitch_min+g.pitch_max) * 100/2); + 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); 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.get_pid(); + float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw); 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.get_pid(), -g.yaw_range * 100/2, g.yaw_range * 100/2); + 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); SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, yaw_out); }