AP_Landing: compute wind_alignment and half of head-wind

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

libraries/AP_Landing/AP_Landing.cpp

@@ -236,3 +236,74 @@ bool AP_Landing::restart_landing_sequence()
     return success;
 }
 
+/*
+ * 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_Landing::wind_alignment(const float heading_deg)
+{
+    const Vector3f wind = ahrs.wind_estimate();
+    const float wind_heading_rad = atan2f(-wind.y, -wind.x);
+    return cosf(wind_heading_rad - radians(heading_deg));
+}
+
+/*
+ * returns head-wind in m/s, 0 for tail-wind.
+ */
+float AP_Landing::head_wind(void)
+{
+    const float alignment = wind_alignment(ahrs.yaw_sensor*0.01f);
+
+    if (alignment <= 0) {
+        return 0;
+    }
+
+    return alignment * ahrs.wind_estimate().length();
+}
+
+/*
+ * returns target airspeed in cm/s depending on flight stage
+ */
+int32_t AP_Landing::get_target_airspeed_cm(const AP_SpdHgtControl::FlightStage flight_stage)
+{
+    int32_t target_airspeed_cm = aparm.airspeed_cruise_cm;
+
+    if (!in_progress) {
+        // not landing, use regular cruise airspeed
+        return target_airspeed_cm;
+    }
+
+    // we're landing, check for custom approach and
+    // pre-flare airspeeds. Also increase for head-winds
+
+    const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
+
+    switch (flight_stage) {
+    case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
+        if (land_airspeed >= 0) {
+            target_airspeed_cm = land_airspeed * 100;
+        }
+        break;
+
+    case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
+    case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
+        if (pre_flare && get_pre_flare_airspeed() > 0) {
+            // if we just preflared then continue using the pre-flare airspeed during final flare
+            target_airspeed_cm = get_pre_flare_airspeed() * 100;
+        } else if (land_airspeed >= 0) {
+            target_airspeed_cm = land_airspeed * 100;
+        }
+        break;
+
+    default:
+        break;
+    }
+
+    // when landing, add half of head-wind.
+    const int32_t head_wind_compensation_cm = head_wind() * 0.5f * 100;
+
+    // Do not lower it or exceed cruise speed
+    return constrain_int32(target_airspeed_cm + head_wind_compensation_cm, target_airspeed_cm, aparm.airspeed_cruise_cm);
+}

libraries/AP_Landing/AP_Landing.h

@@ -86,6 +86,10 @@ public:
 
     static const struct AP_Param::GroupInfo var_info[];
 
+    float wind_alignment(const float heading_deg);
+    float head_wind(void);
+    int32_t get_target_airspeed_cm(const AP_SpdHgtControl::FlightStage flight_stage);
+
     // accessor functions for the params
     int16_t get_pitch_cd(void) const { return pitch_cd; }
     float get_flare_sec(void) const { return flare_sec; }