Add Wind Compensation to turn_distance function

libraries/AP_L1_Control/AP_L1_Control.cpp

@@ -123,11 +123,26 @@ int32_t AP_L1_Control::target_bearing_cd(void) const
  */
 float AP_L1_Control::turn_distance(float wp_radius) const
 {
-    float eas2tas_sq = sq(_ahrs.get_EAS2TAS());
+    float eas2tas_sq = sq(_ahrs.get_EAS2TAS()); // true airspeed correction
+
+    float scaled_turn_distance = wp_radius * eas2tas_sq; // distance scaled to true airspeed
+
+    /** Wind effect i.e. forward crosstrack error correction for the leg to the next waypoint.
+     *  turn early on tail-wind
+     */
+    float headwind = _ahrs.head_wind(); // forward head-wind component in m/s. Negative means tail-wind.
+
+    float arc_distance_for_90_degree_turn_in_meters = 0.5f * 3.14f * wp_radius; // TODO: is there a definition of PI somewhere?
+
+    float ground_speed = _ahrs.groundspeed_vector().length();
+
+    float seconds_needed_for_90_degree_turn = arc_distance_for_90_degree_turn_in_meters / ground_speed;
 
-    float scaled_turn_distance = wp_radius * eas2tas_sq;
+    float estimated_lateral_shift_by_wind_during_turn = time_needed_for_90_degree_turn * head_wind;
 
-    float sanitized_turn_distance = MIN(scaled_turn_distance, _L1_dist);
+    float scaled_and_wind_corrected_turn_distance = scaled_turn_distance - estimated_lateral_shift_by_wind_during_turn;
+
+    float sanitized_turn_distance = MIN(scaled_and_wind_corrected_turn_distance, _L1_dist); // presumably some invariant of the L1 algo
 
     return sanitized_turn_distance;
 }