From a2a3403efaf55e4a06a3ac367b8153e106ef06c3 Mon Sep 17 00:00:00 2001
From: fly <merspieler@airmail.cc>
Date: Thu, 29 Apr 2021 21:18:20 +0000
Subject: [PATCH] Basic sidestick priority

Signed-off-by: fly <merspieler@airmail.cc>
---
 Nasal/FMGC/FCU.nas | 20 ++++++++++++++++++--
 1 file changed, 18 insertions(+), 2 deletions(-)

diff --git a/Nasal/FMGC/FCU.nas b/Nasal/FMGC/FCU.nas
index 000bce55..304484ca 100644
--- a/Nasal/FMGC/FCU.nas
+++ b/Nasal/FMGC/FCU.nas
@@ -34,6 +34,7 @@ var vsModeInput = props.globals.getNode("/it-autoflight/input/vs", 1);
 var locArm = props.globals.getNode("/it-autoflight/output/loc-armed", 1);
 var apprArm = props.globals.getNode("/it-autoflight/output/appr-armed", 1);
 var FCUworkingNode = props.globals.initNode("/FMGC/FCU-working", 0, "BOOL");
+var SidestickPriorityPressedLast = 0;
 
 var FCU = {
 	elecSupply: "",
@@ -146,7 +147,8 @@ var FCUController = {
 			}
 		}
 	},
-	APDisc: func() {
+	APDisc: func(side = 0, press = 0) {
+		# side: 0 = none, 1 = capt, 2 = fo
 		# physical button sound - so put it outside here as you get a sound even if it doesn't work!
 		setprop("/sim/sounde/apdiscbtn", 1);
 		settimer(func {
@@ -156,7 +158,7 @@ var FCUController = {
 		if (me.FCUworking) {
 			if (ap1.getBoolValue() or ap2.getBoolValue()) {
 				apOff("soft", 0);
-			} else {
+			} else if (getprop("/it-autoflight/sound/apoffsound") == 1 or getprop("/it-autoflight/sound/apoffsound2") == 1 or getprop("/it-autoflight/output/ap-warning") != 0) {
 				if (getprop("/it-autoflight/sound/apoffsound") == 1 or getprop("/it-autoflight/sound/apoffsound2") == 1) {
 					setprop("/it-autoflight/sound/apoffsound", 0);
 					setprop("/it-autoflight/sound/apoffsound2", 0);
@@ -165,6 +167,20 @@ var FCUController = {
 					setprop("/it-autoflight/output/ap-warning", 0);
 					ecam.lights[0].setValue(0);
 				}
+			} else if (side != 0) {
+				if (press == 1) {
+					setprop("/fdm/jsbsim/fbw/sidestick/active[" ~ (2 - side) ~ "]", 0);
+					setprop("/fdm/jsbsim/fbw/sidestick/active[" ~ (side - 1) ~ "]", 1);
+					SidestickPriorityPressedLast = side;
+				} else {
+					# Only release, if this side has pressed the button last
+					# to avoide the first pressed side getting activated again
+					# when released.
+					if (SidestickPriorityPressedLast == side) {
+						setprop("/fdm/jsbsim/fbw/sidestick/active[0]", 1);
+						setprop("/fdm/jsbsim/fbw/sidestick/active[1]", 1);
+					}
+				}
 			}
 		}
 	},