From 6e33987a91812d7e3b0f7ec04625c2e2df3add09 Mon Sep 17 00:00:00 2001
From: Josh Davidson <joshdavidson2000@outlook.com>
Date: Fri, 14 Oct 2022 23:43:57 -0400
Subject: [PATCH] Control:  New speed on pitch for generic autopilot

---
 Aircraft/Generic/generic-autopilot.xml | 322 +++++++++++++++++--------
 1 file changed, 225 insertions(+), 97 deletions(-)

diff --git a/Aircraft/Generic/generic-autopilot.xml b/Aircraft/Generic/generic-autopilot.xml
index 91b0640fa..050c1920c 100644
--- a/Aircraft/Generic/generic-autopilot.xml
+++ b/Aircraft/Generic/generic-autopilot.xml
@@ -34,6 +34,9 @@ pitch-limit-max
 
 pitch-limit-min
 - Set the minimum pitch angle in degrees
+
+speed-by-pitch-accel
+- Set the maximum acceleration rate for speed on pitch
 -->
 
 <PropertyList>
@@ -423,10 +426,26 @@ pitch-limit-min
 		<output>/autopilot/internal/mach</output>
 	</filter>
 	
+	<filter> <!-- Filter knots -->
+		<name>Knots Lag</name>
+		<type>exponential</type>
+		<input>/velocities/airspeed-kt</input> <!-- This should NOT change no matter what, do not use the current knots prop -->
+		<output>/autopilot/internal/kts-input-lag</output>
+		<filter-time>0.04</filter-time>
+	</filter>
+	
+	<filter> <!-- Average knots -->
+		<name>Knots Average</name>
+		<type>moving-average</type>
+		<input>/autopilot/internal/kts-input-lag</input>
+		<output>/autopilot/internal/kts-input-avg</output>
+		<samples>60</samples>
+	</filter>
+	
 	<filter> <!-- Compute knots rate -->
 		<name>Knots Rate</name>
 		<type>derivative</type>
-		<input>/velocities/airspeed-kt</input> <!-- This should NOT change no matter what, do not use the current knots prop -->
+		<input>/autopilot/internal/kts-input-avg</input>
 		<output>/autopilot/internal/kts-rate-raw</output>
 		<filter-time>1.0</filter-time>
 	</filter>
@@ -436,7 +455,64 @@ pitch-limit-min
 		<type>exponential</type>
 		<input>/autopilot/internal/kts-rate-raw</input>
 		<output>/autopilot/internal/kts-rate</output>
-		<filter-time>0.08</filter-time>
+		<filter-time>0.04</filter-time>
+	</filter>
+	
+	<filter> <!-- Filter mach -->
+		<name>Mach Lag</name>
+		<type>exponential</type>
+		<input>/velocities/mach</input> <!-- This should NOT change no matter what, do not use the current mach prop -->
+		<output>/autopilot/internal/mach-input-lag</output>
+		<filter-time>0.04</filter-time>
+	</filter>
+	
+	<filter> <!-- Average mach -->
+		<name>Mach Average</name>
+		<type>moving-average</type>
+		<input>/autopilot/internal/mach-input-lag</input>
+		<output>/autopilot/internal/mach-input-avg</output>
+		<samples>60</samples>
+	</filter>
+	
+	<filter> <!-- Compute mach rate -->
+		<name>Mach Rate</name>
+		<type>derivative</type>
+		<input>/autopilot/internal/mach-input-avg</input>
+		<output>/autopilot/internal/mach-rate-raw</output>
+		<filter-time>1.0</filter-time>
+	</filter>
+	
+	<filter> <!-- Multiply mach rate -->
+		<name>Mach Rate Gain</name>
+		<type>gain</type>
+		<gain>650</gain>
+		<input>/autopilot/internal/mach-rate-raw</input>
+		<output>/autopilot/internal/mach-rate-gain</output>
+	</filter>
+	
+	<filter> <!-- Filter mach rate -->
+		<name>Mach Rate Lag</name>
+		<type>exponential</type>
+		<input>/autopilot/internal/mach-rate-raw</input>
+		<output>/autopilot/internal/mach-rate</output>
+		<filter-time>0.04</filter-time>
+	</filter>
+	
+	<filter> <!-- Acceleration limit for speed on pitch -->
+		<name>Flch Acceleration Limit</name>
+		<type>gain</type>
+		<gain>1.0</gain>
+		<input>
+			<condition>
+				<greater-than>
+					<property>/autopilot/config/speed-by-pitch-accel</property>
+					<value>0</value>
+				</greater-than>
+			</condition>
+			<property>/autopilot/config/speed-by-pitch-accel</property>
+		</input>
+		<input>1.5</input>
+		<output>/autopilot/internal/flch-accel</output>
 	</filter>
 	
 	<filter> <!-- Knots rate computer for speed on pitch -->
@@ -456,7 +532,7 @@ pitch-limit-min
 						<property>/autopilot/settings/target-speed-mach</property>
 						<property>/autopilot/internal/mach</property>
 					</difference>
-					<value>80</value>
+					<value>100</value>
 				</product>
 			</expression>
 		</input>
@@ -467,13 +543,16 @@ pitch-limit-min
 						<property>/autopilot/settings/target-speed-kt</property>
 						<property>/autopilot/internal/airspeed-kt</property>
 					</difference>
-					<value>0.08</value>
+					<value>0.1</value>
 				</product>
 			</expression>
 		</input>
 		<output>/autopilot/internal/flch-kts-error-cmd</output>
-		<min>-1.5</min>
-		<max>1.5</max>
+		<min>
+			<property>/autopilot/internal/flch-accel</property>
+			<scale>-1</scale>
+		</min>
+		<max>/autopilot/internal/flch-accel</max>
 	</filter>
 	
 	<filter> <!-- Knots rate command filter -->
@@ -493,7 +572,7 @@ pitch-limit-min
 					</not-equals>
 				</and>
 			</condition>
-			<value>2.5</value>
+			<value>1.5</value>
 		</max-rate-of-change>
 		<max-rate-of-change>100</max-rate-of-change>
 	</filter>
@@ -610,7 +689,7 @@ pitch-limit-min
 							<property>/velocities/airspeed-kt</property>
 							<entry><ind> 80</ind><dep>0.016</dep></entry>
 							<entry><ind>360</ind><dep>0.007</dep></entry>
-							<entry><ind>600</ind><dep>0.002</dep></entry>
+							<entry><ind>600</ind><dep>0.003</dep></entry>
 						</table>
 						<property>/autopilot/internal/gain-factor</property>
 					</product>
@@ -788,59 +867,150 @@ pitch-limit-min
 		<max-rate-of-change>10000</max-rate-of-change>
 	</filter>
 	
-	<pid-controller> <!-- Speed on pitch target fpm controller -->
-		<name>IT-CONTROLLER: Speed By Pitch</name>
-		<enable>
+	<filter> <!-- Speed on pitch target fpm delta controller -->
+		<name>IT-CONTROLLER: Speed By Pitch Delta</name>
+		<type>gain</type>
+		<gain>
+			<expression>
+				<table>
+					<property>/velocities/airspeed-kt</property>
+					<entry><ind>140</ind><dep> 700</dep></entry>
+					<entry><ind>360</ind><dep>1400</dep></entry>
+					<entry><ind>600</ind><dep>2100</dep></entry>
+				</table>
+			</expression>
+		</gain>
+		<input>
+			<condition>
+				<equals>
+					<property>/autopilot/locks/speed</property>
+					<value>mach-with-pitch-trim</value>
+				</equals>
+			</condition>
+			<property>/autopilot/internal/mach-rate</property>
+		</input>
+		<input>/autopilot/internal/kts-rate</input>
+		<reference>/autopilot/internal/flch-kts-error</reference>
+		<output>/autopilot/internal/flch-raw</output>
+		<min>
+			<condition>
+				<or>
+					<equals>
+						<property>/gear/gear[1]/wow</property>
+						<value>1</value>
+					</equals>
+					<equals>
+						<property>/gear/gear[2]/wow</property>
+						<value>1</value>
+					</equals>
+				</or>
+			</condition>
+			<value>0</value>
+		</min>
+		<min>-2000</min>
+		<max>2000</max>
+	</filter>
+	
+	<filter> <!-- Filter speed on pitch target fpm delta -->
+		<name>Speed By Pitch Filter</name>
+		<type>noise-spike</type>
+		<input>/autopilot/internal/flch-raw</input>
+		<output>/autopilot/internal/flch-filtered</output>
+		<max-rate-of-change>
 			<condition>
 				<property>/autopilot/internal/speed-on-pitch-active</property>
 			</condition>
-		</enable>
-		<input>/autopilot/internal/kts-rate</input>
-		<reference>/autopilot/internal/flch-kts-error</reference>
-		<output>/autopilot/internal/target-fpm-flch</output>
-		<config>
-			<Kp>
-				<expression>
-					<table>
-						<property>/velocities/airspeed-kt</property>
-						<entry><ind> 80</ind><dep>-380</dep></entry>
-						<entry><ind>360</ind><dep>-430</dep></entry>
-						<entry><ind>600</ind><dep>-470</dep></entry>
-					</table>
-				</expression>
-			</Kp>
-			<Ti>1.3</Ti>
-			<Td>0.0</Td>
-			<u_min>
-				<expression>
-					<difference>
-						<property>/autopilot/internal/vertical-speed-fpm</property>
-						<value>2000</value>
-					</difference>
-				</expression>
-			</u_min>
-			<u_max>
-				<expression>
-					<sum>
-						<property>/autopilot/internal/vertical-speed-fpm</property>
-						<value>2000</value>
-					</sum>
-				</expression>
-			</u_max>
-		</config>
-	</pid-controller>
+			<value>1500</value>
+		</max-rate-of-change>
+		<max-rate-of-change>10000</max-rate-of-change>
+	</filter>
 	
-	<filter> <!-- Smooth engagement by syncing when disabled -->
-		<name>Speed By Pitch Inactive Sync</name>
+	<filter> <!-- Compute speed on pitch target fpm -->
+		<name>Speed By Pitch Final</name>
 		<type>gain</type>
 		<gain>1.0</gain>
-		<enable>
-			<condition>
-				<not><property>/autopilot/internal/speed-on-pitch-active</property></not>
-			</condition>
-		</enable>
-		<input>/autopilot/internal/vertical-speed-fpm</input>
+		<input>
+			<expression>
+				<sum>
+					<property>/autopilot/internal/vert-speed-fpm</property>
+					<property>/autopilot/internal/flch-filtered</property>
+				</sum>
+			</expression>
+		</input>
 		<output>/autopilot/internal/target-fpm-flch</output>
+		<min>
+			<condition>
+				<or>
+					<equals>
+						<property>/gear/gear[1]/wow</property>
+						<value>1</value>
+					</equals>
+					<equals>
+						<property>/gear/gear[2]/wow</property>
+						<value>1</value>
+					</equals>
+				</or>
+			</condition>
+			<value>0</value>
+		</min>
+		<min>-100000</min>
+		<max>100000</max>
+	</filter>
+	
+	<filter> <!-- Filter target fpm -->
+		<name>Target FPM Filter</name>
+		<type>noise-spike</type>
+		<input> <!-- Must be on top of the others -->
+			<condition>
+				<property>/autopilot/internal/speed-on-pitch-active</property>
+			</condition>
+			<property>/autopilot/internal/target-fpm-flch</property>
+		</input>
+		<input>
+			<condition>
+				<equals>
+					<property>/autopilot/locks/altitude</property>
+					<value>altitude-hold</value>
+				</equals>
+			</condition>
+			<property>/autopilot/internal/target-fpm-alt</property>
+		</input>
+		<input>
+			<condition>
+				<equals>
+					<property>/autopilot/locks/altitude</property>
+					<value>agl-hold</value>
+				</equals>
+			</condition>
+			<property>/autopilot/internal/target-fpm-agl</property>
+		</input>
+		<input>
+			<condition>
+				<equals>
+					<property>/autopilot/locks/altitude</property>
+					<value>fpa-hold</value>
+				</equals>
+			</condition>
+			<property>/autopilot/internal/target-fpm-fpa</property>
+		</input>
+		<input>
+			<condition>
+				<equals>
+					<property>/autopilot/locks/altitude</property>
+					<value>gs1-hold</value>
+				</equals>
+			</condition>
+			<property>/autopilot/internal/nav1-rate-of-climb-fpm</property>
+		</input>
+		<input>/autopilot/internal/target-fpm-vs</input>
+		<output>/autopilot/internal/target-fpm</output>
+		<max-rate-of-change>
+			<condition>
+				<property>/autopilot/internal/vs-active</property>
+			</condition>
+			<value>700</value>
+		</max-rate-of-change>
+		<max-rate-of-change>10000</max-rate-of-change>
 	</filter>
 	
 	<pid-controller> <!-- Main fpm controller -->
@@ -851,49 +1021,7 @@ pitch-limit-min
 			</condition>
 		</enable>
 		<input>/autopilot/internal/vertical-speed-fpm</input>
-		<reference> <!-- Must be on top of the others -->
-			<condition>
-				<property>/autopilot/internal/speed-on-pitch-active</property>
-			</condition>
-			<property>/autopilot/internal/target-fpm-flch</property>
-		</reference>
-		<reference>
-			<condition>
-				<equals>
-					<property>/autopilot/locks/altitude</property>
-					<value>altitude-hold</value>
-				</equals>
-			</condition>
-			<property>/autopilot/internal/target-fpm-alt</property>
-		</reference>
-		<reference>
-			<condition>
-				<equals>
-					<property>/autopilot/locks/altitude</property>
-					<value>agl-hold</value>
-				</equals>
-			</condition>
-			<property>/autopilot/internal/target-fpm-agl</property>
-		</reference>
-		<reference>
-			<condition>
-				<equals>
-					<property>/autopilot/locks/altitude</property>
-					<value>fpa-hold</value>
-				</equals>
-			</condition>
-			<property>/autopilot/internal/target-fpm-fpa</property>
-		</reference>
-		<reference>
-			<condition>
-				<equals>
-					<property>/autopilot/locks/altitude</property>
-					<value>gs1-hold</value>
-				</equals>
-			</condition>
-			<property>/autopilot/internal/nav1-rate-of-climb-fpm</property>
-		</reference>
-		<reference>/autopilot/internal/target-fpm-vs</reference>
+		<reference>/autopilot/internal/target-fpm</reference>
 		<output>/autopilot/internal/target-pitch-deg</output>
 		<config>
 			<Kp>