From 671708d11d4c571e6c88d948775d73b5fb400477 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 3 Aug 2014 18:16:37 +1000
Subject: [PATCH] AntennaTracker: added YAW_TRIM and PITCH_TRIM

---
 Tools/AntennaTracker/Parameters.h   |  4 ++++
 Tools/AntennaTracker/Parameters.pde | 16 ++++++++++++++++
 Tools/AntennaTracker/tracking.pde   | 12 +++++++-----
 3 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/Tools/AntennaTracker/Parameters.h b/Tools/AntennaTracker/Parameters.h
index 0e752d290e..57504182f3 100644
--- a/Tools/AntennaTracker/Parameters.h
+++ b/Tools/AntennaTracker/Parameters.h
@@ -86,6 +86,8 @@ public:
         k_param_onoff_pitch_rate,
         k_param_onoff_yaw_mintime,
         k_param_onoff_pitch_mintime,
+        k_param_yaw_trim,
+        k_param_pitch_trim,
 
         k_param_channel_yaw = 200,
         k_param_channel_pitch,
@@ -128,6 +130,8 @@ public:
     AP_Float onoff_pitch_rate;
     AP_Float onoff_yaw_mintime;
     AP_Float onoff_pitch_mintime;
+    AP_Float yaw_trim;
+    AP_Float pitch_trim;
 
     // Waypoints
     //
diff --git a/Tools/AntennaTracker/Parameters.pde b/Tools/AntennaTracker/Parameters.pde
index edb1b9bd26..e0a42c7920 100644
--- a/Tools/AntennaTracker/Parameters.pde
+++ b/Tools/AntennaTracker/Parameters.pde
@@ -161,6 +161,22 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Range: 0 2
     GSCALAR(onoff_pitch_mintime,   "ONOFF_PITCH_MINT", 0.1f),
 
+    // @Param: YAW_TRIM
+    // @DisplayName: Yaw trim
+    // @Description: Amount of extra yaw to add when tracking. This allows for small adjustments for an out of trim compass.
+    // @Units: degrees
+    // @Increment: 0.1
+    // @Range: -10 10
+    GSCALAR(yaw_trim,              "YAW_TRIM", 0),
+
+    // @Param: PITCH_TRIM
+    // @DisplayName: Pitch trim
+    // @Description: Amount of extra pitch to add when tracking. This allows for small adjustments for a badly calibrated barometer.
+    // @Units: degrees
+    // @Increment: 0.1
+    // @Range: -10 10
+    GSCALAR(pitch_trim,              "PITCH_TRIM", 0),
+
     // barometer ground calibration. The GND_ prefix is chosen for
     // compatibility with previous releases of ArduPlane
     // @Group: GND_
diff --git a/Tools/AntennaTracker/tracking.pde b/Tools/AntennaTracker/tracking.pde
index 8cdbdecfe7..8c2f74ce85 100644
--- a/Tools/AntennaTracker/tracking.pde
+++ b/Tools/AntennaTracker/tracking.pde
@@ -254,16 +254,16 @@ static void update_yaw_onoff_servo(float yaw)
 /**
    update the yaw (azimuth) servo. 
  */
-static void update_yaw_servo(float pitch)
+static void update_yaw_servo(float yaw)
 {
     switch ((enum ServoType)g.servo_type.get()) {
     case SERVO_TYPE_ONOFF:
-        update_yaw_onoff_servo(pitch);
+        update_yaw_onoff_servo(yaw);
         break;
 
     case SERVO_TYPE_POSITION:
     default:
-        update_yaw_position_servo(pitch);
+        update_yaw_position_servo(yaw);
         break;
     }
     channel_yaw.calc_pwm();
@@ -280,8 +280,10 @@ static void update_auto(void)
         hal.scheduler->millis() - start_time_ms < g.startup_delay*1000) {
         return;
     }
-    update_pitch_servo(nav_status.pitch);
-    update_yaw_servo(nav_status.bearing);
+    float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100) * 0.01f;
+    float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90);
+    update_pitch_servo(pitch);
+    update_yaw_servo(yaw);
 }