From de33779382c0b2949c9b4a64ddb128740fda85d9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 21 Apr 2016 21:52:25 +1000
Subject: [PATCH] Plane: added Q_WVANE_MINROLL

this allows for some roll trim without weathervaning
---
 ArduPlane/quadplane.cpp | 23 +++++++++++++++++++++--
 ArduPlane/quadplane.h   |  1 +
 2 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index b540c1101ab95..0c37d477d7190 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -241,11 +241,19 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
 
     // @Param: WVANE_GAIN
     // @DisplayName: Weathervaning gain
-    // @Description: This controls the tendency to yaw to face into the wind. A value of 0.4 is good for reasonably quick wind direction correction.
+    // @Description: This controls the tendency to yaw to face into the wind. A value of 0.4 is good for reasonably quick wind direction correction. The weathervaning works by turning into the direction of roll.
     // @Range: 0 1
     // @Increment: 0.01
     // @User: Standard
     AP_GROUPINFO("WVANE_GAIN", 33, QuadPlane, weathervane.gain, 0),
+
+    // @Param: WVANE_MINROLL
+    // @DisplayName: Weathervaning min roll
+    // @Description: This set the minimum roll in degrees before active weathervaning will start. This may need to be larger if your aircraft has bad roll trim.
+    // @Range: 0 10
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("WVANE_MINROLL", 34, QuadPlane, weathervane.min_roll, 1),
     
     AP_GROUPEND
 };
@@ -1471,7 +1479,18 @@ float QuadPlane::get_weathervane_yaw_rate_cds(void)
         return 0;
     }
 
-    float output = constrain_float((wp_nav->get_roll() / 4500.0f) * weathervane.gain, -1, 1);
+    float roll = wp_nav->get_roll() / 100.0f;
+    if (fabsf(roll) < weathervane.min_roll) {
+        weathervane.last_output = 0;
+        return 0;        
+    }
+    if (roll > 0) {
+        roll -= weathervane.min_roll;
+    } else {
+        roll += weathervane.min_roll;
+    }
+    
+    float output = constrain_float((roll/45.0f) * weathervane.gain, -1, 1);
     if (should_relax()) {
         output = 0;
     }
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 90d91d8ef5180..8ac3ee2d96976 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -184,6 +184,7 @@ class QuadPlane
 
     struct {
         AP_Float gain;
+        AP_Float min_roll;
         uint32_t last_pilot_input_ms;
         float last_output;
     } weathervane;