From 7f2600d01e36a3ba4ebefd029031c4171a348f37 Mon Sep 17 00:00:00 2001
From: Matthias Blankertz <matthias@blankertz.org>
Date: Tue, 6 Jun 2023 22:10:23 +0200
Subject: [PATCH] lua: Fix VS/pitch mode changes for DH8D

---
 FlyWithLua/rpi2040_hid.lua | 61 +++++++++++++++++++++-----------------
 1 file changed, 34 insertions(+), 27 deletions(-)

diff --git a/FlyWithLua/rpi2040_hid.lua b/FlyWithLua/rpi2040_hid.lua
index f3cdab6..a1d4358 100644
--- a/FlyWithLua/rpi2040_hid.lua
+++ b/FlyWithLua/rpi2040_hid.lua
@@ -182,31 +182,59 @@ else
       if app_report_good and app_report[2] ~= 0xffff then
 	 ap_alt = app_report[2]
       end
-      -- vertical speed
+      -- vertical speed or pitch
       local enter_mode_vs = false
       local exit_mode_vs = false
+      local enter_mode_pitch = false
+      local exit_mode_pitch = false
+      local mode_vs = false
+      local mode_pitch = false
       if ac_icao == "DH8D" then
 	 if q4xp_status_altmode == 3 and prev_status_altmode ~= 3 then
 	    enter_mode_vs = true
 	 elseif q4xp_status_altmode ~= 3 and prev_status_altmode == 3 then
 	    exit_mode_vs = true
 	 end
+	 if q4xp_status_altmode == 3 then
+	    mode_vs = true
+	 end
+	 if q4xp_status_altmode == 1 and prev_status_altmode ~= 1 then
+	    enter_mode_pitch = true
+	 elseif q4xp_status_altmode ~= 1 and prev_status_altmode == 1 then
+	    exit_mode_pitch = true
+	 end
+	 if q4xp_status_altmode == 1 then
+	    mode_pitch = true
+	 end
       else
 	 if status_altmode == 4 and prev_status_altmode ~= 4 then
 	    enter_mode_vs = true
 	 elseif status_altmode ~= 4 and prev_status_altmode == 4 then
 	    exit_mode_vs = true
 	 end
+	 if status_altmode == 4 then
+	    mode_vs = true
+	 end
       end
       if enter_mode_vs then
 	 send_vs = math.floor(cur_vvi + 0.5)
-      elseif exit_mode_vs then
+      elseif enter_mode_pitch then
+	 send_vs = math.floor(q4xp_ap_pitch + 0.5)*100
+      elseif exit_mode_vs or exit_mode_pitch then
 	 send_vs = -2
       elseif app_report_good and app_report[3] ~= 0xffff then
-	 if bit.band(app_report[3], 0x8000) ~= 0 then
-	    ap_vs = -bit.band(bit.bnot(app_report[3]), 0x7fff) - 1
-	 else
-	    ap_vs = app_report[3]
+	 if mode_vs then
+	    if bit.band(app_report[3], 0x8000) ~= 0 then
+	       ap_vs = -bit.band(bit.bnot(app_report[3]), 0x7fff) - 1
+	    else
+	       ap_vs = app_report[3]
+	    end
+	 elseif mode_pitch then
+	    if bit.band(app_report[3], 0x8000) ~= 0 then
+	       q4xp_ap_pitch = (-bit.band(bit.bnot(app_report[3]), 0x7fff) - 1)/100
+	    else
+	       q4xp_ap_pitch = app_report[3]/100
+	    end
 	 end
       end
       -- airspeed
@@ -257,27 +285,6 @@ else
 	    ap_as_kt_mach = app_report[4]
 	 end
       end
-      --pitch
-      local enter_mode_pitch = false
-      local exit_mode_pitch = false
-      if ac_icao == "DH8D" then
-	 if q4xp_status_altmode == 1 and prev_status_altmode ~= 1 then
-	    enter_mode_pitch = true
-	 elseif q4xp_status_altmode ~= 1 and prev_status_altmode == 1 then
-	    exit_mode_pitch = true
-	 end
-	 if enter_mode_pitch then
-	    send_vs = math.floor(q4xp_ap_pitch + 0.5)*100
-	 elseif exit_mode_pitch then
-	    send_vs = -2
-	 elseif app_report_good and app_report[3] ~= 0xffff then
-	    if bit.band(app_report[3], 0x8000) ~= 0 then
-	       q4xp_ap_pitch = (-bit.band(bit.bnot(app_report[3]), 0x7fff) - 1)/100
-	    else
-	       q4xp_ap_pitch = app_report[3]/100
-	    end
-	 end
-      end
       send_app_report(device, send_hdg, build_leds(), send_alt, send_vs, send_as, status_machmode == 1)
       if ac_icao == "DH8D" then
 	 prev_status_altmode = q4xp_status_altmode