From 9ef10703decda95fa75d03bb365b5a036127e68e Mon Sep 17 00:00:00 2001
From: Airwide <arvid.stenberg@gmail.com>
Date: Sat, 5 Sep 2020 23:26:34 +0200
Subject: [PATCH] Added deadband to fixedWingPitchToThrottleCorrection for
 testing

---
 src/main/navigation/navigation_fixedwing.c | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index ae700e3c1f..8cb99e94d5 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -463,7 +463,20 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
 
 int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
 {
-    return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
+    const int16_t pitch_to_throttle_deadband = 30;
+
+    if (pitch > pitch_to_throttle_deadband) {
+        // Above positive pitch deadband
+        return DECIDEGREES_TO_DEGREES(pitch - pitch_to_throttle_deadband) * navConfig()->fw.pitch_to_throttle;
+    }
+    else if (pitch < -pitch_to_throttle_deadband) {
+        // Below negative pitch deadband
+        return DECIDEGREES_TO_DEGREES(pitch + pitch_to_throttle_deadband) * navConfig()->fw.pitch_to_throttle;
+    }
+    else {   
+        // Inside deadband
+        return 0;
+    }
 }
 
 void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)