diff --git a/AntennaTracker/control_auto.cpp b/AntennaTracker/control_auto.cpp index 1d9ebc2cf0..ef7725ce60 100644 --- a/AntennaTracker/control_auto.cpp +++ b/AntennaTracker/control_auto.cpp @@ -60,6 +60,12 @@ void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed) convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err); nav_status.angle_error_pitch = bf_pitch_err; 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_actual_rate(ahrs_pitch * 0.01); + g.pidYaw2Srv.set_desired_rate(yaw * 0.01); + g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01); } void Tracker::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)