diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index 014dcaee7c..23e1e0cab5 100644 --- a/AntennaTracker/servos.cpp +++ b/AntennaTracker/servos.cpp @@ -74,7 +74,7 @@ void Tracker::update_pitch_position_servo() // PITCH2SRV_IMAX 4000.000000 // calculate new servo position - float new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch); + float new_servo_out = SRV_Channels::get_output_scaled(SRV_Channel::k_tracker_pitch) + g.pidPitch2Srv.update_error(nav_status.angle_error_pitch, G_Dt); // position limit pitch servo if (new_servo_out <= pitch_min_cd) { @@ -125,7 +125,7 @@ void Tracker::update_pitch_onoff_servo(float pitch) const */ void Tracker::update_pitch_cr_servo(float 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.update_error(nav_status.angle_error_pitch, G_Dt), -(-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); } @@ -187,7 +187,7 @@ void Tracker::update_yaw_position_servo() right direction */ - float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw); + float servo_change = g.pidYaw2Srv.update_error(nav_status.angle_error_yaw, G_Dt); 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); @@ -238,6 +238,6 @@ void Tracker::update_yaw_onoff_servo(float yaw) const */ void Tracker::update_yaw_cr_servo(float 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.update_error(nav_status.angle_error_yaw, G_Dt), -g.yaw_range * 100/2, g.yaw_range * 100/2); SRV_Channels::set_output_scaled(SRV_Channel::k_tracker_yaw, yaw_out); }