From 9c2ff8e51dd5dcfd5ee290d03d49938cd89dde99 Mon Sep 17 00:00:00 2001
From: Joshua Davidson <joshuadavidson2000@gmail.com>
Date: Tue, 1 Aug 2017 19:29:44 -0400
Subject: [PATCH] A3XX: Fix oscilating FBW at high Mach, with dynamically
 scaled Kp for Pitch Rate, and Roll Rate PIDs

---
 Systems/it-fbw.xml | 40 ++++++++++++++++++++++++++++++++++++++--
 1 file changed, 38 insertions(+), 2 deletions(-)

diff --git a/Systems/it-fbw.xml b/Systems/it-fbw.xml
index a185b681..f45519ce 100644
--- a/Systems/it-fbw.xml
+++ b/Systems/it-fbw.xml
@@ -327,6 +327,22 @@
 		<output>/it-fbw/roll-rate</output>
     </filter>
 	
+	<filter>
+		<name>IT-FBW: ROLL KP (P Gain)</name>
+		<type>gain</type>
+		<gain>1</gain>
+		<input>
+			<expression>
+				<table>
+					<property>/velocities/mach</property>
+					<entry><ind>0.2</ind><dep>0.11</dep></entry>
+					<entry><ind>0.9</ind><dep>0.07</dep></entry>
+				</table>
+			</expression>
+		</input>
+		<output>/it-fbw/roll-kp</output>
+	</filter>
+	
 	<pid-controller>
 		<name>IT-FBW: ROLL</name>
 		<debug>false</debug>
@@ -358,7 +374,9 @@
 			<property>/controls/flight/aileron-fbw-cmd</property>
 		</output>
 		<config>
-			<Kp>0.11</Kp>
+			<Kp>
+				<property>/it-fbw/roll-kp</property>
+			</Kp>
 			<beta>1</beta>
 			<alpha>0.1</alpha>
 			<gamma>0</gamma>
@@ -833,6 +851,22 @@
 		<output>/it-fbw/pitch-rate</output>
     </filter>
 	
+	<filter>
+		<name>IT-FBW: PITCH KP (P Gain)</name>
+		<type>gain</type>
+		<gain>1</gain>
+		<input>
+			<expression>
+				<table>
+					<property>/velocities/mach</property>
+					<entry><ind>0.2</ind><dep>-0.20</dep></entry>
+					<entry><ind>0.9</ind><dep>-0.13</dep></entry>
+				</table>
+			</expression>
+		</input>
+		<output>/it-fbw/pitch-kp</output>
+	</filter>
+	
 	<pid-controller>
 		<name>IT-FBW: PITCH</name>
 		<debug>false</debug>
@@ -882,7 +916,9 @@
 			<property>/controls/flight/elevator-fbw-cmd</property>
 		</output>
 		<config>
-			<Kp>-0.20</Kp>
+			<Kp>
+				<property>/it-fbw/pitch-kp</property>
+			</Kp>
 			<beta>1</beta>
 			<alpha>0.1</alpha>
 			<gamma>0</gamma>