Initial working setup for SR22

FlyWithLua/rpi2040_hid.lua

@@ -1,20 +1,165 @@
-device = hid_open(0x1209, 1)
+local bit = require("bit")
 
+all_devs, number = create_HID_table()
+local the_index
+for k, v in pairs(all_devs) do
+   if v.vendor_id == 0x1209 and v.usage == 0 then
+      the_path = v.path
+      --print(v.path)
+      --print(v.usage)
+   end
+end
+
+local button_offset = 320
+local button_hdg_sync = 1
+local button_alt_sync = 7
+local led_hdg  = 0x0001
+local led_apr  = 0x0002
+local led_nav  = 0x0004
+local led_lvl  = 0x0008
+local led_alt  = 0x0010
+local led_fd   = 0x0020
+local led_vnav = 0x0040
+local led_ap   = 0x0080
+local led_vs   = 0x0100
+local led_at   = 0x0200
+local led_flc  = 0x0400
+local led_spd  = 0x0800
+local led_n1   = 0x1000
+
+function table.clone(org)
+  return {table.unpack(org)}
+end
+
+function send_app_report(device, hdg, leds, alt, vs, as)
+   hid_write(device, 2, 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
+
+function build_leds()
+   local val = 0
+   if status_hdg == 1 then
+      val = val + led_hdg
+   elseif status_hdg == 2 then
+      val = val + led_nav
+   end
+   if status_apr > 0 then
+      val = val + led_apr
+   end
+   if status_alt > 1 then
+      val = val + led_alt
+   end
+   if status_fd == 1 then
+      val = val + led_fd
+   elseif status_fd == 2 then
+      val = val + led_fd + led_ap
+   end
+   if status_vnav > 1 then
+      val = val + led_vnav
+   end
+   if status_altmode == 4 then
+      val = val + led_vs
+   elseif status_altmode == 5 then
+      val = val + led_flc
+   end
+      
+   return val
+end
+
+if the_path ~= nil then
+   device = hid_open_path(the_path)
+end
 if device == nil then
    print("No device!")
 else
-   dataref("low_vac", "sim/cockpit2/annunciators/low_vacuum", "readable")
-   local prev_low_vac
-   function low_vac_indicator()
-      if low_vac ~= prev_low_vac then
-	 if low_vac > 0 then
-	    hid_write(device, 2, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
-	 else
-	    hid_write(device, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
+   hid_set_nonblocking(device, 1)
+   dataref("ap_hdg", "sim/cockpit2/autopilot/heading_dial_deg_mag_pilot", "writable")
+   dataref("mag_hdg", "sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_copilot")
+
+   dataref("ap_alt", "sim/cockpit2/autopilot/altitude_dial_ft", "writable")
+   dataref("cur_alt", "sim/cockpit2/gauges/indicators/altitude_ft_pilot")
+
+   dataref("ap_vs", "sim/cockpit2/autopilot/vvi_dial_fpm", "writable")
+   dataref("cur_vvi", "sim/cockpit2/gauges/indicators/vvi_fpm_pilot")
+
+   dataref("ap_as", "sim/cockpit2/autopilot/airspeed_dial_kts", "writable")
+   dataref("cur_ias", "sim/cockpit2/gauges/indicators/airspeed_kts_pilot")
+   
+   dataref("status_alt", "sim/cockpit2/autopilot/altitude_hold_status")
+   dataref("status_apr", "sim/cockpit2/autopilot/approach_status")
+   dataref("status_hdg", "sim/cockpit2/autopilot/heading_mode")
+   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)
+   local prev_status_altmode = 0
+   function usb_app()
+      local send_hdg = -1
+      local send_alt = -1
+      local send_vs = -1
+      local send_as = -1
+      local app_report = {0xffff, 0xffff, 0xffff, 0xffff}
+      local app_report_good = false
+      local loops = 100
+      -- read app report from hid device, discarding all reports except the latest one to prevent input lag
+      repeat
+	 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] = hid_read(device, 11)
+	 if tmp_cnt >= 11 and tmp[0] == 2 then
+	    app_report = {bit.bor(tmp[1], bit.lshift(tmp[2], 8)),
+			  bit.bor(tmp[5], bit.lshift(tmp[6], 8)),
+			  bit.bor(tmp[7], bit.lshift(tmp[8], 8)),
+			  bit.bor(tmp[9], bit.lshift(tmp[10], 8))}
+	    app_report_good = true
 	 end
-	 prev_low_vac = low_vac
+	 loops = loops - 1
+      until tmp_cnt == 0 or tmp_cnt == nil or loops == 0
+      -- autopilot heading
+      if button(button_offset + button_hdg_sync) and not last_button(button_offset + button_hdg_sync) then
+	 mag_hdg_int = math.floor(mag_hdg + 0.5)
+	 ap_hdg = mag_hdg_int
+	 send_hdg = mag_hdg_int
+      elseif app_report_good and app_report[1] ~= 0xffff then
+	 ap_hdg = app_report[1]
       end
+      -- autopilot altitude
+      if button(button_offset + button_alt_sync) and not last_button(button_offset + button_alt_sync) then
+	 cur_alt_int = math.floor(cur_alt + 0.5)
+	 ap_alt = cur_alt_int
+	 send_alt = cur_alt_int
+      end
+      if app_report_good and app_report[2] ~= 0xffff then
+	 ap_alt = app_report[2]
+      end
+      -- vertical speed
+      if status_altmode == 4 and prev_status_altmode ~= 4 then
+	 send_vs = math.floor(cur_vvi + 0.5)
+      elseif status_altmode ~= 4 and prev_status_altmode == 4 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]
+	 end
+      end
+      -- airspeed
+      if status_altmode == 5 and prev_status_altmode ~= 5 then
+	 if cur_ias < 64 then
+	    send_as = 64
+	 else
+	    send_as = math.floor(cur_ias + 0.5)
+	 end
+      elseif status_altmode ~= 5 and prev_status_altmode == 5 then
+	 send_as = -2
+      elseif app_report_good and app_report[4] ~= 0xffff then
+	 ap_as = app_report[4]
+      end
+      send_app_report(device, send_hdg, build_leds(), send_alt, send_vs, send_as)
+      prev_status_altmode = status_altmode
    end
 
-   do_every_frame("low_vac_indicator()")
+   do_every_frame("usb_app()")
 end