APM: added XTRK_USE_WIND parameter

when enabled this will use the wind estimation code to adjust the
navigation bearing, allowing the navigation code to cope with much
higher levels of wind while using a compass

ArduPlane/Parameters.h

@@ -119,6 +119,7 @@ public:
         k_param_RTL_altitude_cm,
         k_param_inverted_flight_ch,
         k_param_min_gndspeed_cm,
+        k_param_crosstrack_use_wind,
 
 
         //
@@ -232,6 +233,7 @@ public:
     //
     AP_Float crosstrack_gain;
     AP_Int16 crosstrack_entry_angle;
+    AP_Int8  crosstrack_use_wind;
 
     // Estimation
     //

ArduPlane/Parameters.pde

@@ -112,6 +112,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD",  XTRACK_ENTRY_ANGLE_CENTIDEGREE),
 
+    // @Param: XTRK_USE_WIND
+    // @DisplayName: Crosstrack Wind correction
+    // @Description: If enabled, use wind estimation for navigation crosstrack when using a compass for yaw
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    GSCALAR(crosstrack_use_wind, "XTRK_USE_WIND",  1),
+
     // @Param: ALT_MIX
     // @DisplayName: Gps to Baro Mix
     // @Description: The percent of mixing between gps altitude and baro altitude. 0 = 100% gps, 1 = 100% baro

ArduPlane/commands_logic.pde

@@ -355,9 +355,9 @@ static bool verify_land()
         nav_bearing_cd = hold_course;
         // recalc bearing error
         calc_bearing_error();
+    } else {
+        update_crosstrack();
     }
-
-    update_crosstrack();
     return false;
 }
 

ArduPlane/navigation.pde

@@ -173,14 +173,29 @@ static void update_loiter()
 
 static void update_crosstrack(void)
 {
+    // if we are using a compass for navigation, then adjust the
+    // heading to account for wind
+    if (g.crosstrack_use_wind && compass.use_for_yaw()) {
+        Vector3f wind = ahrs.wind_estimate();
+        Vector2f wind2d = Vector2f(wind.x, wind.y);
+        float speed;
+        if (ahrs.airspeed_estimate(&speed)) {
+            Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed;
+            Vector2f nav_adjusted = nav_vector - wind2d;
+            nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100;
+        }
+    }
+
     // Crosstrack Error
     // ----------------
     // If we are too far off or too close we don't do track following
     if (abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) {
-        crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;               // Meters we are off track line
+        // Meters we are off track line
+        crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;               
         nav_bearing_cd += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
         nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
     }
+
 }
 
 static void reset_crosstrack()