From 4e69288995d2580fb1fc6b3db8a9dde4bdee7578 Mon Sep 17 00:00:00 2001
From: Jonathan Redpath <legoboyvdlp@gmail.com>
Date: Fri, 28 Jan 2022 16:49:43 +0000
Subject: [PATCH] IRS align: improve conditions to use accurate lateral
 velocity parameters

---
 Nasal/Systems/ADIRS/ADR.nas |  8 ++------
 Systems/a320-adr.xml        | 28 ++++++++++++++++++++++++++++
 Systems/libraries.xml       |  8 ++++++++
 3 files changed, 38 insertions(+), 6 deletions(-)

diff --git a/Nasal/Systems/ADIRS/ADR.nas b/Nasal/Systems/ADIRS/ADR.nas
index 6987df3d..a0c36f52 100644
--- a/Nasal/Systems/ADIRS/ADR.nas
+++ b/Nasal/Systems/ADIRS/ADR.nas
@@ -144,13 +144,9 @@ var ADIRU = {
 		}
 	},
 	_excessMotion: 0,
+	alignFault: props.globals.getNode("/systems/navigation/align-fault"),
 	alignLoop: func() {
-		me._roll = pts.Orientation.roll.getValue();
-		me._pitch = pts.Orientation.pitch.getValue();
-		me._gs = pts.Velocities.groundspeed.getValue();
-		
-		# todo use IR values
-		if (me._gs > 5 or abs(me._pitch) > 5 or abs(me._roll) > 10) {
+		if (me.alignFault.getBoolValue()) {
 			me.stopAlignNoAlign();
 			me._excessMotion = 1;
 			me.update(); # update operative
diff --git a/Systems/a320-adr.xml b/Systems/a320-adr.xml
index bf207379..02baba04 100644
--- a/Systems/a320-adr.xml
+++ b/Systems/a320-adr.xml
@@ -4,6 +4,34 @@
 
 <system name="A320: ADR">
 
+	<channel name="ADR Align Fault" execrate="8">
+	
+		<lag_filter name="velocities/r-aero-deg_sec_filter">
+			<input>velocities/r-aero-deg_sec</input>
+			<c1>1</c1>
+		</lag_filter>
+		
+		<lag_filter name="velocities/v-north-fps_filter">
+			<input>velocities/v-north-fps</input>
+			<c1>1</c1>
+		</lag_filter>
+		
+		<lag_filter name="velocities/v-east-fps_filter">
+			<input>velocities/v-east-fps</input>
+			<c1>1</c1>
+		</lag_filter>
+		
+		<switch name="/systems/navigation/align-fault">
+			<default value="0"/>
+			<test logic="OR" value="1">
+				velocities/r-aero-deg_sec_filter ge 2
+				velocities/v-north-fps_filter ge 0.5
+				velocities/v-east-fps_filter ge 0.5
+			</test>
+		</switch>
+		
+	</channel>
+	
     <channel name="ADR Inputs" execrate="2">
 		
 		<fcs_function name="aero/alpha-deg-corrected">
diff --git a/Systems/libraries.xml b/Systems/libraries.xml
index 35cbed5a..4c7cc82b 100644
--- a/Systems/libraries.xml
+++ b/Systems/libraries.xml
@@ -23,6 +23,14 @@
 		<filter-gain>0.0</filter-gain>
 	</predict-simple>
 
+	<filter>
+		<name>Heading Rate Degrees Per Second</name>
+		<type>gain</type>
+		<gain>57.2957795131</gain>
+		<input>/fdm/jsbsim/velocities/r-aero-rad_sec</input>
+		<output>/fdm/jsbsim/velocities/r-aero-deg_sec</output>
+	</filter>
+	
 	<filter>
 		<name>Gear AGL Altitude FT</name>
 		<type>gain</type>