AntennaTracker: added YAW_TRIM and PITCH_TRIM

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
     //

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_

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);
 }