From a56eac0274d64d7e6426528afe9ecb05d0691f5d Mon Sep 17 00:00:00 2001
From: Peter Hall <iampete@hotmail.co.uk>
Date: Sun, 23 Jan 2022 22:23:37 +0000
Subject: [PATCH] AC_AttitudeControl: WeatherVane: add takeoff and landing
 override params

---
 .../AC_AttitudeControl/AC_WeatherVane.cpp     | 29 ++++++++++++++++++-
 libraries/AC_AttitudeControl/AC_WeatherVane.h |  4 ++-
 2 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp
index e74a8565ea..3db207935e 100644
--- a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp
+++ b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp
@@ -71,6 +71,20 @@ const AP_Param::GroupInfo AC_WeatherVane::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("VELZ_MAX", 6, AC_WeatherVane, _max_vel_z, WVANE_PARAM_VELZ_MAX_DEFAULT),
 
+    // @Param: TAKEOFF
+    // @DisplayName: Takeoff override
+    // @Description: Override the weather vaning behaviour when in takeoffs
+    // @Values: -1:No override,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
+    // @User: Standard
+    AP_GROUPINFO("TAKEOFF", 7, AC_WeatherVane, _takeoff_direction, -1),
+
+    // @Param: LAND
+    // @DisplayName: Landing override
+    // @Description: Override the weather vaning behaviour when in landing
+    // @Values: -1:No override,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
+    // @User: Standard
+    AP_GROUPINFO("LAND", 8, AC_WeatherVane, _landing_direction, -1),
+
     AP_GROUPEND
 };
 
@@ -81,7 +95,7 @@ AC_WeatherVane::AC_WeatherVane(void)
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg)
+bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing)
 {
     Direction dir = (Direction)_direction.get();
     if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0)) {
@@ -92,6 +106,19 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con
         return false;
     }
 
+    // override direction when in takeoff for landing
+    if (is_takeoff && (_takeoff_direction >= 0)) {
+        dir = (Direction)_takeoff_direction.get();
+    }
+    if (is_landing && (_landing_direction >= 0)) {
+        dir = (Direction)_landing_direction.get();
+    }
+    if (dir == Direction::OFF) {
+        // Disabled for takeoff or landing
+        reset();
+        return false;
+    }
+
     // Check if we are above the minimum height to weather vane
     if (is_positive(_min_height) && (hgt <= _min_height)) {
         reset();
diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.h b/libraries/AC_AttitudeControl/AC_WeatherVane.h
index a6cf35139b..b0cb57f8a2 100644
--- a/libraries/AC_AttitudeControl/AC_WeatherVane.h
+++ b/libraries/AC_AttitudeControl/AC_WeatherVane.h
@@ -10,7 +10,7 @@ class AC_WeatherVane {
         CLASS_NO_COPY(AC_WeatherVane);
 
         // Calculate and return the yaw output to weathervane the vehicle
-        bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg);
+        bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing);
 
         // Function to reset all flags and set values. Invoked whenever the weather vaning process is interrupted
         void reset(void);
@@ -38,6 +38,8 @@ class AC_WeatherVane {
         AP_Float _min_height;
         AP_Float _max_vel_xy;
         AP_Float _max_vel_z;
+        AP_Int8 _landing_direction;
+        AP_Int8 _takeoff_direction;
 
         float last_output;
         bool active_msg_sent;