diff --git a/FlyWithLua/rpi2040_hid.lua b/FlyWithLua/rpi2040_hid.lua index 46f0614..97eb904 100644 --- a/FlyWithLua/rpi2040_hid.lua +++ b/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()") diff --git a/src/main.c b/src/main.c index 8f92fa0..81be171 100644 --- a/src/main.c +++ b/src/main.c @@ -24,6 +24,9 @@ __attribute__((packed)) struct button_report { uint32_t buttons; }; +#define APP_MODE_MASK 0x0f +#define APP_FLAG_MACH 0x10 + #define APP_UPDATE 0 #define APP_INIT 1 @@ -40,7 +43,8 @@ __attribute__((packed)) struct app_report { struct __attribute__((packed)) { int16_t min_as; int16_t max_as; - int16_t reserved[3]; + int16_t max_mach; + int16_t reserved[2]; } init; }; }; @@ -48,13 +52,14 @@ __attribute__((packed)) struct app_report { static struct light_data the_light_data; static bool show_vs = false, show_as = false; +static bool mach_mode = false; static volatile unsigned hdg = 0; static volatile unsigned alt = 0; static volatile int vs = 0; static volatile unsigned as = 0; -static int min_as = 0, max_as = 999; +static int min_as = 0, max_as = 999, max_mach = 990; #define BUTTON_HDG_SYNC 1 #define BUTTON_ALT_SYNC 7 @@ -109,12 +114,20 @@ static void update_as_display(void) { if (show_as) { int val = as; - for (int i = 0; i < 4; ++i) { - if (val || i == 0) + if (mach_mode) { + sevenseg_set_digit(12, 0 | LED_DECIMAL); + for (int i = 0; i < 3; ++i) { sevenseg_set_digit(15 - i, val % 10); - else - sevenseg_set_digit(15 - i, LED_BLANK); - val /= 10; + val /= 10; + } + } else { + for (int i = 0; i < 4; ++i) { + if (val || i == 0) + sevenseg_set_digit(15 - i, val % 10); + else + sevenseg_set_digit(15 - i, LED_BLANK); + val /= 10; + } } } else { for (int i = 0; i < 4; ++i) { @@ -123,6 +136,11 @@ static void update_as_display(void) } } +static int16_t clamp(int16_t cur, int16_t min, int16_t max) +{ + return cur >= min ? (cur <= max ? cur : max) : min; +} + void rotary_event(int rot_ch, bool ccw) { /* if (ccw) */ @@ -154,11 +172,18 @@ void rotary_event(int rot_ch, bool ccw) } update_vs_display(); } else { - // TODO: knots / mach mode switch - if (ccw) { - as = as <= min_as ? min_as : as - 1; + if (mach_mode) { + if (ccw) { + as = clamp(((as + 9) / 10) * 10 - 10, 0, max_mach); + } else { + as = clamp((as / 10) * 10 + 10, 0, max_mach); + } } else { - as = as >= max_as ? max_as : as + 1; + if (ccw) { + as = clamp(as - 1, min_as, max_as); + } else { + as = clamp(as + 1, min_as, max_as); + } } update_as_display(); } @@ -202,7 +227,8 @@ static void hid_task(void) start_ms += interval_ms; if (start_ms % 10 == 0 && send_heading_reports) { - struct app_report report = { .type = APP_UPDATE, .update = { .hdg = -1, .alt = -1, .vs = -1, .as = -1 } }; + struct app_report report = { .type = APP_UPDATE | mach_mode ? APP_FLAG_MACH : 0, + .update = { .hdg = -1, .alt = -1, .vs = -1, .as = -1 } }; if (send_heading_reports) report.update.hdg = hdg; if (send_alt_reports) @@ -239,6 +265,7 @@ static void hid_task(void) static void app_update_report(struct app_report const *report) { + mach_mode = report->type & APP_FLAG_MACH; if (report->update.hdg >= 0) { hdg = report->update.hdg; update_hdg_display(); @@ -281,8 +308,10 @@ static void app_init_report(struct app_report const *report) { min_as = report->init.min_as; max_as = report->init.max_as; + max_mach = report->init.max_mach; show_vs = false; show_as = false; + mach_mode = false; } void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_t report_type, uint8_t const *buffer, @@ -292,7 +321,7 @@ void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_ if (report_id == HID_REPORT_APP && report_type == HID_REPORT_TYPE_OUTPUT && bufsize >= 2) { struct app_report report; memcpy(&report, buffer, sizeof(report)); - switch (report.type) { + switch (report.type & APP_MODE_MASK) { case APP_UPDATE: app_update_report(&report); break;