From 3a9ad24907863f19a44fce21221123d1ee699b3a Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Wed, 30 Nov 2016 10:05:25 -0800
Subject: [PATCH] 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 | 71 +++++++++++++++++++++++++++++
 libraries/AP_Landing/AP_Landing.h   |  4 ++
 2 files changed, 75 insertions(+)

diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp
index b88cef1ec0..30aa164b4d 100644
--- a/libraries/AP_Landing/AP_Landing.cpp
+++ b/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);
+}
diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h
index b97e2b266d..eac10be619 100644
--- a/libraries/AP_Landing/AP_Landing.h
+++ b/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; }