From 392b59d7d629a3949cc8e79d01f7b921af7954d8 Mon Sep 17 00:00:00 2001 From: IamPete1 <33176108+IamPete1@users.noreply.github.com> Date: Sun, 10 Mar 2019 10:39:23 +0000 Subject: [PATCH] Tracker: set actual and desired for GCS-PID message --- AntennaTracker/control_auto.cpp | 6 ++++++ 1 file changed, 6 insertions(+) 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)