AP_AHRS: move wind helper functions from AP_Landing

libraries/AP_AHRS/AP_AHRS.cpp

@@ -812,6 +812,32 @@ bool AP_AHRS::_wind_estimate(Vector3f &wind) const
     return false;
 }
 
+
+/*
+ * Determine how aligned heading_deg is with the wind. Return result
+ * is 1.0 when perfectly aligned heading into wind, -1 when perfectly
+ * aligned with-wind, and zero when perfect cross-wind. There is no
+ * distinction between a left or right cross-wind. Wind speed is ignored
+ */
+float AP_AHRS::wind_alignment(const float heading_deg) const
+{
+    Vector3f wind;
+    if (!wind_estimate(wind)) {
+        return 0;
+    }
+    const float wind_heading_rad = atan2f(-wind.y, -wind.x);
+    return cosf(wind_heading_rad - radians(heading_deg));
+}
+
+/*
+ * returns forward head-wind component in m/s. Negative means tail-wind.
+ */
+float AP_AHRS::head_wind(void) const
+{
+    const float alignment = wind_alignment(yaw_sensor*0.01f);
+    return alignment * wind_estimate().length();
+}
+
 /*
   return true if the current AHRS airspeed estimate is directly derived from an airspeed sensor
  */

libraries/AP_AHRS/AP_AHRS.h

@@ -114,6 +114,14 @@ class AP_AHRS {
     // return a wind estimation vector, in m/s; returns 0,0,0 on failure
     bool wind_estimate(Vector3f &wind) const;
 
+    // Determine how aligned heading_deg is with the wind. Return result
+    // is 1.0 when perfectly aligned heading into wind, -1 when perfectly
+    // aligned with-wind,
+    float wind_alignment(const float heading_deg) const;
+
+    // returns forward head-wind component in m/s. Negative means tail-wind.
+    float head_wind(void) const;
+
     // instruct DCM to update its wind estimate:
     void estimate_wind() {
 #if AP_AHRS_DCM_ENABLED