Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: set actual and desired for GCS-PID message

This commit is contained in:
IamPete1 2019-03-10 10:39:23 +00:00 committed by Peter Barker
parent 144e3ed39a
commit 392b59d7d6
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
AntennaTracker/control_auto.cpp
View File

@ -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)