Add support for mach mode and CSC for E55P

FlyWithLua/rpi2040_hid.lua

@@ -28,23 +28,31 @@ local led_spd  = 0x0800
 local led_n1   = 0x1000
 
 local aircraft_data = {
-   ["SR22"] = {["min_as"] = 64, ["max_as"] = 205},
-   ["DR40"] = {["min_as"] = 49, ["max_as"] = 146},
-   ["E55P"] = {["min_as"] = 89, ["max_as"] = 320},
+   ["SR22"] = {["min_as"] = 64, ["max_as"] = 205, ["has_mach"] = false, ["has_csc"] = false},
+   ["DR40"] = {["min_as"] = 49, ["max_as"] = 146, ["has_mach"] = false, ["has_csc"] = false},
+   ["E55P"] = {["min_as"] = 89, ["max_as"] = 320, ["has_mach"] = true, ["max_mach"] = 0.78, ["has_csc"] = true},
 }
 
 dataref("ac_icao", "sim/aircraft/view/acf_ICAO")
 
-print("Aircraft is " .. ac_icao)
 local cur_aircraft = aircraft_data[ac_icao]
-print("Min AS " .. cur_aircraft.min_as .. ", Max AS " .. cur_aircraft.max_as)
+if cur_aircraft == nil then
+   print("Unknown Aircraft " .. ac_icao)
+   cur_aircraft = {["min_as"] = 0, ["max_as"] = 999, ["has_mach"] = true, ["max_mach"] = 0.99,  ["has_csc"] = false}
+else
+   print("Aircraft is " .. ac_icao .. ": Min AS " .. cur_aircraft.min_as .. ", Max AS " .. cur_aircraft.max_as)
+end
 
 function table.clone(org)
   return {table.unpack(org)}
 end
 
-function send_app_report(device, hdg, leds, alt, vs, as)
-   hid_write(device, 2, 0, bit.band(hdg, 0xff), bit.arshift(hdg, 8), bit.band(leds, 0xff), bit.arshift(leds, 8),
+function send_app_report(device, hdg, leds, alt, vs, as, mach)
+   type_ = 0
+   if mach then
+      type_ = bit.bor(type_, 0x10)
+   end
+   hid_write(device, 2, type_, bit.band(hdg, 0xff), bit.arshift(hdg, 8), bit.band(leds, 0xff), bit.arshift(leds, 8),
 	     bit.band(alt, 0xff), bit.arshift(alt, 8), bit.band(vs, 0xff), bit.arshift(vs, 8),
 	     bit.band(as, 0xff), bit.arshift(as, 8))
 end
@@ -81,13 +89,23 @@ end
 
 if the_path ~= nil then
    device = hid_open_path(the_path)
+else
+   print("Device not found!")
+   device = hid_open(0x1209, 0x1)
 end
 if device == nil then
    print("No device!")
 else
    hid_set_nonblocking(device, 1)
-   hid_write(device, 2, 1, bit.band(cur_aircraft.min_as, 0xff), bit.arshift(cur_aircraft.min_as, 8),
-	  bit.band(cur_aircraft.max_as, 0xff), bit.arshift(cur_aircraft.max_as, 8), 0, 0, 0, 0, 0, 0)
+   if cur_aircraft.has_mach then
+      local max_mach = math.floor(cur_aircraft.max_mach * 1000 + 0.5)
+      hid_write(device, 2, 1, bit.band(cur_aircraft.min_as, 0xff), bit.arshift(cur_aircraft.min_as, 8),
+		bit.band(cur_aircraft.max_as, 0xff), bit.arshift(cur_aircraft.max_as, 8),
+		bit.band(max_mach, 0xff), bit.arshift(max_mach, 8), 0, 0, 0, 0)
+   else
+      hid_write(device, 2, 1, bit.band(cur_aircraft.min_as, 0xff), bit.arshift(cur_aircraft.min_as, 8),
+		bit.band(cur_aircraft.max_as, 0xff), bit.arshift(cur_aircraft.max_as, 8), 0, 0, 0, 0, 0, 0)
+   end
    dataref("ap_hdg", "sim/cockpit2/autopilot/heading_dial_deg_mag_pilot", "writable")
    dataref("mag_hdg", "sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_copilot")
 
@@ -98,7 +116,9 @@ else
    dataref("cur_vvi", "sim/cockpit2/gauges/indicators/vvi_fpm_pilot")
 
    dataref("ap_as", "sim/cockpit2/autopilot/airspeed_dial_kts", "writable")
+   dataref("ap_as_kt_mach", "sim/cockpit2/autopilot/airspeed_dial_kts_mach", "writable")
    dataref("cur_ias", "sim/cockpit2/gauges/indicators/airspeed_kts_pilot")
+   dataref("cur_mach", "sim/cockpit2/gauges/indicators/mach_pilot")
    
    dataref("status_alt", "sim/cockpit2/autopilot/altitude_hold_status")
    dataref("status_apr", "sim/cockpit2/autopilot/approach_status")
@@ -106,8 +126,10 @@ else
    dataref("status_fd", "sim/cockpit2/autopilot/flight_director_mode")
    dataref("status_vnav", "sim/cockpit2/autopilot/vnav_status")
    dataref("status_altmode", "sim/cockpit2/autopilot/altitude_mode")
-   send_app_report(device, math.floor(ap_hdg + 0.5), build_leds(), math.floor(ap_alt + 0.5), -2, -2)
+   dataref("status_machmode", "sim/cockpit2/autopilot/airspeed_is_mach")
+   send_app_report(device, math.floor(ap_hdg + 0.5), build_leds(), math.floor(ap_alt + 0.5), -2, -2, status_machmode == 1)
    local prev_status_altmode = 0
+   local prev_status_machmode = status_machmode
    function usb_app()
       local send_hdg = -1
       local send_alt = -1
@@ -121,7 +143,7 @@ else
 	 local tmp_cnt
 	 local tmp = {}
 	 tmp_cnt, tmp[0], tmp[1], tmp[2], tmp[3], tmp[4], tmp[5], tmp[6], tmp[7], tmp[8], tmp[9], tmp[10], tmp[11] = hid_read(device, 12)
-	 if tmp_cnt >= 11 and tmp[0] == 2 and tmp[1] == 0 then
+	 if tmp_cnt >= 11 and tmp[0] == 2 and bit.band(tmp[1], 0xf) == 0 then
 	    app_report = {bit.bor(tmp[2], bit.lshift(tmp[3], 8)),
 			  bit.bor(tmp[6], bit.lshift(tmp[7], 8)),
 			  bit.bor(tmp[8], bit.lshift(tmp[9], 8)),
@@ -161,18 +183,33 @@ else
       end
       -- airspeed
       if status_altmode == 5 and prev_status_altmode ~= 5 then
-	 if cur_ias < cur_aircraft.min_as then
-	    send_as = cur_aircraft.min_as
+	 if status_machmode == 1 then
+	    send_as = math.floor(cur_mach * 1000 + 0.5)
 	 else
-	    send_as = math.floor(cur_ias + 0.5)
+	    if cur_ias < cur_aircraft.min_as then
+	       send_as = cur_aircraft.min_as
+	    else
+	       send_as = math.floor(cur_ias + 0.5)
+	    end
 	 end
       elseif status_altmode ~= 5 and prev_status_altmode == 5 then
 	 send_as = -2
+      elseif status_altmode == 5 and status_machmode ~= prev_status_machmode then
+	 if status_machmode == 1 then
+	    send_as = math.floor(ap_as_kt_mach*1000 + 0.5)
+	 else
+	    send_as = math.floor(ap_as_kt_mach + 0.5)
+	 end
       elseif app_report_good and app_report[4] ~= 0xffff then
-	 ap_as = app_report[4]
+	 if status_machmode == 1 then
+	    ap_as_kt_mach = app_report[4] / 1000
+	 else
+	    ap_as_kt_mach = app_report[4]
+	 end
       end
-      send_app_report(device, send_hdg, build_leds(), send_alt, send_vs, send_as)
+      send_app_report(device, send_hdg, build_leds(), send_alt, send_vs, send_as, status_machmode == 1)
       prev_status_altmode = status_altmode
+      prev_status_machmode = status_machmode
    end
 
    do_every_frame("usb_app()")