Tracker: set actual and desired for GCS-PID message

ArduPilot · Mar 17, 2019 · 392b59d · 392b59d
1 parent 144e3ed
commit 392b59d
Showing 1 changed file with 6 additions and 0 deletions.
diff --git 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)