diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 00000000..b61cdda2 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,4 @@ +*.luac -diff +*.png -diff +*.wav -diff +dist/SCRIPTS/TELEMETRY/iNav.lua -diff \ No newline at end of file diff --git a/.gitignore b/.gitignore index 31c172c1..e9507d5c 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,5 @@ dist/SCRIPTS/TELEMETRY/iNav/*.lua *.dat *.zip desktop.ini -dist/THEMES/ \ No newline at end of file +dist/THEMES/ +obj/ \ No newline at end of file diff --git a/README.md b/README.md index be03606a..216827b9 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -## INAV Lua Telemetry Flight Status for Taranis/Horus/Jumper T16 - v1.7.0 +## INAV Lua Telemetry Flight Status for Taranis/Horus/Jumper T16 - v1.7.1 ### FrSky SmartPort(S.Port), D-series, F.Port & TBS Crossfire telemetry on Taranis, Horus & Jumper T16 transmitters @@ -35,13 +35,13 @@ ## Features * Works with all FrSky telemetry receivers (X, R9 and D series), all TBS Crossfire receivers, all FrSky Taranis and Horus transmitters, and the Jumper T16 transmitter -* Compatible with Betaflight using FrSky X or R9 series receivers (with reduced functionality) and TBS Crossfire support with Betaflight >3.5.5 +* Compatible with Betaflight using FrSky X or R9 series receivers (with reduced functionality) and TBS Crossfire support with Betaflight 4.0.0+ * Launch/pilot-based model orientation and location indicators (great for lost orientation/losing sight of your model) * Compass-based direction indicator (with magnetometer sensor on multirotor or fixed-wing with GPS) * Pilot (glass cockpit) view which includes attitude indicator as well as pilot-familiar layout of additional data * Radar (map) view shows model in relationship to home position, can be displayed either as launch/pilot-based or compass-based orientation * Altitude graph view shows altitude for the last 1-6 minutes -* Horus and Jumper T16 transmitters show all views at the same time, and include additional features like roll indicator and uncaged pitch ladder +* Horus and Jumper T16 transmitters show all views at the same time, and include additional features like roll scale * Bar gauges for Fuel (% battery mAh capacity remaining), Battery voltage, RSSI strength, Transmitter battery, GPS accuracy (HDOP), Variometer (and Altitude for X9D, X9D+ and X9E transmitters) * Display and voice alerts for flight modes and flight mode modifiers (altitude hold, heading hold, home reset, etc.) * Voice notifications for % battery remaining (based on current), voltage low/critical, high altitude, lost GPS, ready to arm, armed, disarmed, etc. @@ -55,7 +55,7 @@ ## Requirements -* [OpenTX v2.2.2+](http://www.open-tx.org/) running on Taranis Q X7/Q X7S, X9D/X9D+, X9E, X-Lite, Horus X10/X10S or X12S (OpenTX v2.2.3+ is suggested) +* [OpenTX v2.2.2+](http://www.open-tx.org/) running on Taranis Q X7/Q X7S, X9D/X9D+, X9E, X9 Lite, X-Lite/X-Lite Pro, Horus X10/X10S or X12S (OpenTX v2.2.3+ is suggested) * Jumper T16 requires [JumperTX 2.2.3+](https://www.jumper.xyz/jumpertx-t16) (May 23, 2019 or after release) * FrSky X, R9 or D series telemetry receiver: X4RSB, X8R, XSR, R-XSR, XSR-M, XSR-E, RX4R, RX6R, XM, XM+, R9, R9 Slim, R9 Slim+, R9 Mini, R9 MM, D8R-II plus, D8R-XP, D4R-II, etc. or any TBS Crossfire receiver: Micro, Nano, Diversity, etc. * [INAV v1.7.3+](https://github.com/iNavFlight/inav/releases) running on your flight controller (INAV v2.1.0+ is suggested for full functionality) - Also compatible with Betaflight (with reduced functionality) @@ -90,6 +90,7 @@ * [Download latest release](https://github.com/iNavFlight/LuaTelemetry/releases/latest) * [Installation Instructions](https://github.com/iNavFlight/LuaTelemetry/wiki/Installation) * [Upgrade Instructions](https://github.com/iNavFlight/LuaTelemetry/wiki/Upgrade) +* [Download Options](https://github.com/iNavFlight/LuaTelemetry/wiki/Download-Options) ## Information & Settings diff --git a/assets/iNavConfigHorus.png b/assets/iNavConfigHorus.png index 1abed914..a7ada0bd 100644 Binary files a/assets/iNavConfigHorus.png and b/assets/iNavConfigHorus.png differ diff --git a/assets/iNavX9Dalt.png b/assets/iNavX9Dalt.png index 90194638..7d479ce2 100644 Binary files a/assets/iNavX9Dalt.png and b/assets/iNavX9Dalt.png differ diff --git a/build b/build new file mode 100644 index 00000000..7fbef866 --- /dev/null +++ b/build @@ -0,0 +1,34 @@ +#!/bin/bash + +echo +echo Compiling INAV Lua Telemetry... +echo + +mkdir obj + +luac52 -s -o obj/main.luac dist/WIDGETS/iNav/main.lua +luac52 -s -o obj/iNav.luac src/iNav.lua +luac52 -s -o obj/alt.luac src/iNav/alt.lua +luac52 -s -o obj/config.luac src/iNav/config.lua +luac52 -s -o obj/crsf.luac src/iNav/crsf.lua +luac52 -s -o obj/data.luac src/iNav/data.lua +luac52 -s -o obj/func_h.luac src/iNav/func_h.lua +luac52 -s -o obj/func_t.luac src/iNav/func_t.lua +luac52 -s -o obj/horus.luac src/iNav/horus.lua +luac52 -s -o obj/lang.luac src/iNav/lang.lua +luac52 -s -o obj/lang_de.luac src/iNav/lang_de.lua +luac52 -s -o obj/lang_es.luac src/iNav/lang_es.lua +luac52 -s -o obj/lang_fr.luac src/iNav/lang_fr.lua +luac52 -s -o obj/load.luac src/iNav/load.lua +luac52 -s -o obj/menu.luac src/iNav/menu.lua +luac52 -s -o obj/modes.luac src/iNav/modes.lua +luac52 -s -o obj/other.luac src/iNav/other.lua +luac52 -s -o obj/pilot.luac src/iNav/pilot.lua +luac52 -s -o obj/radar.luac src/iNav/radar.lua +luac52 -s -o obj/reset.luac src/iNav/reset.lua +luac52 -s -o obj/view.luac src/iNav/view.lua + +rm -fR obj/ + +echo +echo Complete \ No newline at end of file diff --git a/dist/SCRIPTS/TELEMETRY/iNav.lua b/dist/SCRIPTS/TELEMETRY/iNav.lua index 91a023c7..bee2e0f2 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav.lua and b/dist/SCRIPTS/TELEMETRY/iNav.lua differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav.luac b/dist/SCRIPTS/TELEMETRY/iNav.luac index 91a023c7..bee2e0f2 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav.luac and b/dist/SCRIPTS/TELEMETRY/iNav.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/alt.luac b/dist/SCRIPTS/TELEMETRY/iNav/alt.luac index 1490aabb..914b0272 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/alt.luac and b/dist/SCRIPTS/TELEMETRY/iNav/alt.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/config.luac b/dist/SCRIPTS/TELEMETRY/iNav/config.luac index 4cce558e..fdb1d6e6 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/config.luac and b/dist/SCRIPTS/TELEMETRY/iNav/config.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/crsf.luac b/dist/SCRIPTS/TELEMETRY/iNav/crsf.luac index 27e5e9a0..da39df03 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/crsf.luac and b/dist/SCRIPTS/TELEMETRY/iNav/crsf.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/data.luac b/dist/SCRIPTS/TELEMETRY/iNav/data.luac index 1ec86364..ae37f98e 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/data.luac and b/dist/SCRIPTS/TELEMETRY/iNav/data.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/func_h.luac b/dist/SCRIPTS/TELEMETRY/iNav/func_h.luac index 6534f4ee..dd63e4af 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/func_h.luac and b/dist/SCRIPTS/TELEMETRY/iNav/func_h.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/horus.luac b/dist/SCRIPTS/TELEMETRY/iNav/horus.luac index f91a2ccd..f5db5331 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/horus.luac and b/dist/SCRIPTS/TELEMETRY/iNav/horus.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/lang.luac b/dist/SCRIPTS/TELEMETRY/iNav/lang.luac index e9235c4d..fbcd76db 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/lang.luac and b/dist/SCRIPTS/TELEMETRY/iNav/lang.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/lang_de.luac b/dist/SCRIPTS/TELEMETRY/iNav/lang_de.luac index 1d4fca5f..1017b5b5 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/lang_de.luac and b/dist/SCRIPTS/TELEMETRY/iNav/lang_de.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/lang_es.luac b/dist/SCRIPTS/TELEMETRY/iNav/lang_es.luac index 72eb3bc1..fb5e50c2 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/lang_es.luac and b/dist/SCRIPTS/TELEMETRY/iNav/lang_es.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/lang_fr.luac b/dist/SCRIPTS/TELEMETRY/iNav/lang_fr.luac index 2b1f951a..47389d48 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/lang_fr.luac and b/dist/SCRIPTS/TELEMETRY/iNav/lang_fr.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/menu.luac b/dist/SCRIPTS/TELEMETRY/iNav/menu.luac index 5232a39b..e7a2c8d2 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/menu.luac and b/dist/SCRIPTS/TELEMETRY/iNav/menu.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/other.luac b/dist/SCRIPTS/TELEMETRY/iNav/other.luac index 49f8ec95..dc4f86f2 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/other.luac and b/dist/SCRIPTS/TELEMETRY/iNav/other.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/pics/fpv.png b/dist/SCRIPTS/TELEMETRY/iNav/pics/fpv.png new file mode 100644 index 00000000..4a106f87 Binary files /dev/null and b/dist/SCRIPTS/TELEMETRY/iNav/pics/fpv.png differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/pilot.luac b/dist/SCRIPTS/TELEMETRY/iNav/pilot.luac index 801b8224..778a1578 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/pilot.luac and b/dist/SCRIPTS/TELEMETRY/iNav/pilot.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/radar.luac b/dist/SCRIPTS/TELEMETRY/iNav/radar.luac index 2e282714..3ee67de0 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/radar.luac and b/dist/SCRIPTS/TELEMETRY/iNav/radar.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/reset.luac b/dist/SCRIPTS/TELEMETRY/iNav/reset.luac index bcb1aa2b..bfa077c3 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/reset.luac and b/dist/SCRIPTS/TELEMETRY/iNav/reset.luac differ diff --git a/dist/SCRIPTS/TELEMETRY/iNav/view.luac b/dist/SCRIPTS/TELEMETRY/iNav/view.luac index 50399d0f..2c35b001 100644 Binary files a/dist/SCRIPTS/TELEMETRY/iNav/view.luac and b/dist/SCRIPTS/TELEMETRY/iNav/view.luac differ diff --git a/dist/WIDGETS/iNav/main.lua b/dist/WIDGETS/iNav/main.lua index 66f3c4b0..b5266312 100644 --- a/dist/WIDGETS/iNav/main.lua +++ b/dist/WIDGETS/iNav/main.lua @@ -24,6 +24,7 @@ end -- This function allow updates when you change widgets settings local function update(iNavZone, options) iNavZone.options = options + return end -- Called periodically when custom telemetry screen containing widget is visible. diff --git a/dist/WIDGETS/iNav/main.luac b/dist/WIDGETS/iNav/main.luac index e1bf0bea..708d3f7c 100644 Binary files a/dist/WIDGETS/iNav/main.luac and b/dist/WIDGETS/iNav/main.luac differ diff --git a/release b/release index 95a77db0..80bd96ae 100644 --- a/release +++ b/release @@ -1,5 +1,6 @@ #!/bin/bash +echo echo -n Packaging release files rm LuaTelemetry-Taranis-en.zip > /dev/null 2>&1 @@ -10,13 +11,13 @@ echo -n . cd dist -zip -9 -q -r ../LuaTelemetry-Taranis-en.zip SCRIPTS/* -x *.lua -x *.dat -x */pics/* -x */de/* -x */es/* -x */fr/* +zip -9 -q -r ../LuaTelemetry-Taranis-en.zip SCRIPTS/* -x *.lua -x *.dat -x */pics/* -x */horus.luac -x */func_h.luac -x */de/* -x */es/* -x */fr/* -x */lang*.luac echo -n . -zip -9 -q -r ../LuaTelemetry-Taranis.zip SCRIPTS/* -x *.lua -x *.dat -x */pics/* +zip -9 -q -r ../LuaTelemetry-Taranis.zip SCRIPTS/* -x *.lua -x *.dat -x */pics/* -x */horus.luac -x */func_h.luac echo -n . -zip -9 -q -r ../LuaTelemetry-Horus-en.zip SCRIPTS/* WIDGETS/* -x *.lua -x *.dat -x */de/* -x */es/* -x */fr/* +zip -9 -q -r ../LuaTelemetry-Horus-en.zip SCRIPTS/* WIDGETS/* -x *.lua -x *.dat -x */de/* -x */es/* -x */fr/* -x */lang*.luac -x */alt.luac -x */func_t.luac -x */pilot.luac -x */radar.luac -x */view.luac echo -n . -zip -9 -q -r ../LuaTelemetry-Horus.zip SCRIPTS/* WIDGETS/* -x *.lua -x *.dat +zip -9 -q -r ../LuaTelemetry-Horus.zip SCRIPTS/* WIDGETS/* -x *.lua -x *.dat -x */alt.luac -x */func_t.luac -x */pilot.luac -x */radar.luac -x */view.luac echo -n . zip -9 -q -r ../LuaTelemetry.zip SCRIPTS/* WIDGETS/* -x *.lua -x *.dat echo -n . diff --git a/src/iNav.lua b/src/iNav.lua index 68ddc78d..745ffc9a 100644 --- a/src/iNav.lua +++ b/src/iNav.lua @@ -3,29 +3,33 @@ -- Docs: https://github.com/iNavFlight/LuaTelemetry local buildMode = ... -local VERSION = "1.7.0" +local VERSION = "1.7.1" local FILE_PATH = "/SCRIPTS/TELEMETRY/iNav/" local SMLCD = LCD_W < 212 local HORUS = LCD_W >= 480 local FLASH = HORUS and WARNING_COLOR or 3 local tmp, view, lang +local env = "bx" --- Build with Companion +-- Build with Companion and allow debugging local v, r, m, i, e = getVersion() -if string.sub(r, -4) == "simu" and buildMode ~= false then - loadScript(FILE_PATH .. "build", "tx")(buildMode) +if string.sub(r, -4) == "simu" then + env = "tx" + if buildMode ~= false then + loadScript(FILE_PATH .. "build", "tx")(buildMode) + end end -local config = loadfile(FILE_PATH .. "config.luac")(SMLCD) +local config = loadScript(FILE_PATH .. "config", env)(SMLCD) collectgarbage() -local modes, units, labels = loadfile(FILE_PATH .. "modes.luac")() +local modes, units, labels = loadScript(FILE_PATH .. "modes", env)() collectgarbage() -local data, getTelemetryId, getTelemetryUnit, PREV, INCR, NEXT, DECR, MENU = loadfile(FILE_PATH .. "data.luac")(r, m, i, HORUS) +local data, getTelemetryId, getTelemetryUnit, PREV, INCR, NEXT, DECR, MENU = loadScript(FILE_PATH .. "data", env)(r, m, i, HORUS) collectgarbage() -loadfile(FILE_PATH .. "load.luac")(config, data, FILE_PATH) +loadScript(FILE_PATH .. "load", env)(config, data, FILE_PATH) collectgarbage() --[[ Simulator language testing @@ -34,15 +38,17 @@ data.voice = "es" ]] if data.lang ~= "en" or data.voice ~= "en" then - lang = loadfile(FILE_PATH .. "lang.luac")(modes, labels, data, FILE_PATH) + lang = loadScript(FILE_PATH .. "lang", env)(modes, labels, data, FILE_PATH, env) collectgarbage() end -loadfile(FILE_PATH .. "reset.luac")(data) -local crsf = loadfile(FILE_PATH .. "other.luac")(config, data, units, getTelemetryId, getTelemetryUnit, FILE_PATH) +loadScript(FILE_PATH .. "reset", env)(data) +collectgarbage() + +local crsf, distCalc = loadScript(FILE_PATH .. "other", env)(config, data, units, getTelemetryId, getTelemetryUnit, FILE_PATH, env) collectgarbage() -local title, gpsDegMin, hdopGraph, icons, widgetEvt = loadfile(FILE_PATH .. (HORUS and "func_h.luac" or "func_t.luac"))(config, data, FILE_PATH) +local title, gpsDegMin, hdopGraph, icons, widgetEvt = loadScript(FILE_PATH .. (HORUS and "func_h" or "func_t"), env)(config, data, FILE_PATH) collectgarbage() local function playAudio(f, a) @@ -51,24 +57,15 @@ local function playAudio(f, a) end end -local function calcTrig(gps1, gps2, deg) - local o1 = math.rad(gps1.lat) - local a1 = math.rad(gps1.lon) - local o2 = math.rad(gps2.lat) - local a2 = math.rad(gps2.lon) - if deg then - local x = (math.cos(o1) * math.sin(o2)) - (math.sin(o1) * math.cos(o2) * math.cos(a2 - a1)) - local y = math.sin(a2 - a1) * math.cos(o2) - return math.deg(math.atan2(y, x)) - else - --[[ Spherical-Earth math: More accurate but only at extreme distances - return math.acos(math.sin(o1) * math.sin(o2) + math.cos(o1) * math.cos(o2) * math.cos(a2 - a1)) * 6371009; - ]] - -- Flat-Earth math - local x = (a2 - a1) * math.cos((o1 + o2) / 2) - local y = o2 - o1 - return math.sqrt(x * x + y * y) * 6371009 - end +local function calcBearing(gps1, gps2) + --[[ Spherical-Earth math: More accurate if the Earth was a sphere, but obviously it's not + local x = (math.cos(o1) * math.sin(o2)) - (math.sin(o1) * math.cos(o2) * math.cos(a2 - a1)) + local y = math.sin(a2 - a1) * math.cos(o2) + return math.deg(math.atan2(y, x)) + ]] + -- Flat-Earth math + local x = (gps2.lon - gps1.lon) * math.cos(math.rad(gps1.lat)) + return math.deg(1.5708 - math.atan2(gps2.lat - gps1.lat, x)) end local function calcDir(r1, r2, r3, x, y, r) @@ -82,11 +79,11 @@ local function calcDir(r1, r2, r3, x, y, r) end local function background() - data.rssi = getValue(data.rssi_id) + data.rssi, data.rssiLow, data.rssiCrit = getRSSI() if data.rssi > 0 then data.telem = true data.telemFlags = 0 - data.rssiMin = getValue(data.rssiMin_id) > 0 and getValue(data.rssiMin_id) or data.rssiMin + data.rssiMin = math.min(data.rssiMin, data.rssi) data.satellites = getValue(data.sat_id) if data.showFuel then data.fuel = getValue(data.fuel_id) @@ -146,16 +143,8 @@ local function background() if data.satellites > 1000 and gpsTemp.lat ~= 0 and gpsTemp.lon ~= 0 then data.gpsFix = true data.lastLock = gpsTemp - -- Calculate distance to home if sensor is missing or in simlulator - if data.gpsHome ~= false and (data.dist_id == -1 or data.simu) then - data.distance = calcTrig(data.gpsHome, data.gpsLatLon, false) - data.distanceMax = math.max(data.distMaxCalc, data.distance) - data.distMaxCalc = data.distanceMax - -- If distance is in feet, convert - if data.dist_unit == 10 then - data.distance = math.floor(data.distance * 3.28084 + 0.5) - data.distanceMax = data.distanceMax * 3.28084 - end + if data.gpsHome ~= false and distCalc ~= nil then + distCalc(data) end end end @@ -230,6 +219,7 @@ local function background() data.showMax = false data.showDir = config[32].v == 1 and true or false data.configStatus = 0 + data.configSelect = 0 if not data.gpsAltBase and data.gpsFix then data.gpsAltBase = data.gpsAlt end @@ -381,7 +371,7 @@ local function background() -- Initalize variables on flight reset (uses timer3) tmp = model.getTimer(2) if tmp.value == 0 then - loadfile(FILE_PATH .. "reset.luac")(data) + loadScript(FILE_PATH .. "reset", env)(data) tmp.value = 3600 model.setTimer(2, tmp) end @@ -431,19 +421,20 @@ local function run(event) end -- Display error if Horus widget isn't full screen - if data.widget then - if (iNavZone.zone.w < 450 or iNavZone.zone.h < 250) and data.msg ~= false then - lcd.drawText(iNavZone.zone.x + 14, iNavZone.zone.y + 16, data.msg, SMLSIZE + WARNING_COLOR) - data.startupTime = math.huge -- Never timeout - return 0 - end - event = data.armed and 0 or widgetEvt(data) + if data.widget and data.msg ~= false and (iNavZone.zone.w < 450 or iNavZone.zone.h < 250) then + lcd.drawText(iNavZone.zone.x + 14, iNavZone.zone.y + 16, data.msg, SMLSIZE + WARNING_COLOR) + data.startupTime = math.huge -- Never timeout + return 0 end -- Clear screen if HORUS then lcd.setColor(CUSTOM_COLOR, 264) --lcd.RGB(0, 32, 65) lcd.clear(CUSTOM_COLOR) + -- On Horus use sticks to control the menu + if event == 0 or event == nil then + event = widgetEvt(data) + end else lcd.clear() end @@ -459,7 +450,7 @@ local function run(event) if data.v ~= 9 then view = nil collectgarbage() - view = loadfile(FILE_PATH .. "menu.luac")() + view = loadScript(FILE_PATH .. "menu", env)() data.v = 9 end tmp = config[30].v @@ -472,6 +463,10 @@ local function run(event) if data.configStatus == 27 and data.configSelect ~= 0 then icons.sym(icons.fg) end + -- Return throttle stick to bottom center + if data.stickMsg ~= nil and not data.armed then + icons.alert() + end end else -- User input @@ -494,10 +489,10 @@ local function run(event) if data.v ~= config[25].v then view = nil collectgarbage() - view = loadfile(FILE_PATH .. (HORUS and "horus.luac" or (config[25].v == 0 and "view.luac" or (config[25].v == 1 and "pilot.luac" or (config[25].v == 2 and "radar.luac" or "alt.luac")))))() + view = loadScript(FILE_PATH .. (HORUS and "horus" or (config[25].v == 0 and "view" or (config[25].v == 1 and "pilot" or (config[25].v == 2 and "radar" or "alt")))), env)() data.v = config[25].v end - view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcTrig, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) + view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcBearing, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) end collectgarbage() diff --git a/src/iNav/alt.lua b/src/iNav/alt.lua index 0ced7b2d..3daf3a70 100644 --- a/src/iNav/alt.lua +++ b/src/iNav/alt.lua @@ -1,4 +1,4 @@ -local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcTrig, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) +local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcBearing, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) local LEFT_DIV = 36 local LEFT_POS = SMLCD and LEFT_DIV or 73 @@ -61,15 +61,15 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic -- Altitude graph local BOTTOM = SMLCD and 47 or 63 tmp = (SMLCD and 30 or 40) / (data.altMax - data.altMin) - lcd.drawLine(RIGHT_POS - 60, BOTTOM, RIGHT_POS - 1, BOTTOM, SOLID, FORCE) + lcd.drawLine(RIGHT_POS - 60, BOTTOM, RIGHT_POS - 1, BOTTOM, SOLID, SMLCD and FORCE or GREY_DEFAULT + FORCE) for i = 1, 60 do local cx = RIGHT_POS - 61 + i local cy = math.floor(BOTTOM - (data.alt[((data.altCur - 2 + i) % 60) + 1] - data.altMin) * tmp) if cy < BOTTOM then - lcd.drawLine(cx, cy, cx, BOTTOM - 1, SOLID, FORCE) + lcd.drawLine(cx, cy, cx, BOTTOM - 1, SOLID, SMLCD and FORCE or GREY_DEFAULT + FORCE) end if (i ~= 1 or not SMLCD) and (i - 1) % (60 / config[28].v) == 0 then - lcd.drawLine(cx, BOTTOM - (SMLCD and 30 or 40), cx, BOTTOM, DOTTED, SMLCD and 0 or GREY_DEFAULT) + lcd.drawLine(cx, BOTTOM - (SMLCD and 30 or 40), cx, BOTTOM, DOTTED, 0) end end if data.altMin < -1 then @@ -82,7 +82,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic -- Orientation if not SMLCD and data.telem then - if data.showDir or data.headingRef < 0 then + if data.showDir or data.headingRef == -1 then lcd.drawText(LEFT_POS + 12, 29, "N", SMLSIZE) lcd.drawText(LEFT_POS + 25 - (data.heading < 100 and 3 or 0) - (data.heading < 10 and 3 or 0), 57, math.floor(data.heading + 0.5) % 360 .. "\64", SMLSIZE + RIGHT + data.telemFlags) tmp = data.heading @@ -150,14 +150,14 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic lcd.drawLine(LEFT_DIV, 8, LEFT_DIV, 63, SOLID, FORCE) tmp = (not data.telem or data.cell < config[3].v or (data.showFuel and config[23].v == 0 and data.fuel <= config[17].v)) and FLASH or 0 if data.showFuel then - if config[23].v == 0 then + if config[23].v > 0 or (data.crsf and data.showMax) then + lcd.drawText(LEFT_DIV, data.showCurr and 8 or 10, (data.crsf and data.fuelRaw or data.fuel), MIDSIZE + RIGHT + tmp) + lcd.drawText(LEFT_DIV, data.showCurr and 20 or 23, data.fUnit[data.crsf and 1 or config[23].v], SMLSIZE + RIGHT + tmp) + else lcd.drawText(LEFT_DIV - 5, data.showCurr and 8 or 12, data.fuel, DBLSIZE + RIGHT + tmp) lcd.drawText(LEFT_DIV, data.showCurr and 17 or 21, "%", SMLSIZE + RIGHT + tmp) - else - lcd.drawText(LEFT_DIV, data.showCurr and 8 or 10, data.fuel, MIDSIZE + RIGHT + tmp) - lcd.drawText(LEFT_DIV, data.showCurr and 20 or 23, data.fUnit[config[23].v], SMLSIZE + RIGHT + tmp) end - end +end lcd.drawText(LEFT_DIV - 5, data.showCurr and 25 or 32, string.format(config[1].v == 0 and "%.2f" or "%.1f", config[1].v == 0 and (data.showMax and data.cellMin or data.cell) or (data.showMax and data.battMin or data.batt)), DBLSIZE + RIGHT + tmp) lcd.drawText(LEFT_DIV, data.showCurr and 34 or 41, "V", SMLSIZE + RIGHT + tmp) if data.showCurr then diff --git a/src/iNav/build.lua b/src/iNav/build.lua index fd5e1eb9..8c933766 100644 --- a/src/iNav/build.lua +++ b/src/iNav/build.lua @@ -4,7 +4,7 @@ local FLASH = 3 local SMLCD = LCD_W < 212 local HORUS = LCD_W >= 480 local v, r, m, i, e = getVersion() -local env = "tc" -- Default: "tc" | Debug mode: "tcb" +local env = "tc" local config = loadScript(FILE_PATH .. "config", env)(SMLCD) local modes, units, labels = loadScript(FILE_PATH .. "modes", env)() @@ -17,7 +17,7 @@ local title, gpsDegMin, hdopGraph, icons, widgetEvt = loadScript(FILE_PATH .. "f data.lang = "en" data.voice = "en" -local lang = loadScript(FILE_PATH .. "lang", env)(modes, labels, data, FILE_PATH) +local lang = loadScript(FILE_PATH .. "lang", env)(modes, labels, data, FILE_PATH, env) local langs = { "nl", "fr", "it", "de", "cz", "sk", "es", "pl", "pt", "ru", "se", "hu" } local config2 for abv = 1, #langs do @@ -30,7 +30,7 @@ end loadScript(FILE_PATH .. "reset", env)(data) local crsf = loadScript(FILE_PATH .. "crsf", env)(config, data, getTelemetryId) -crsf = loadScript(FILE_PATH .. "other", env)(config, data, units, getTelemetryId, getTelemetryUnit, FILE_PATH) +crsf, distCalc = loadScript(FILE_PATH .. "other", env)(config, data, units, getTelemetryId, getTelemetryUnit, FILE_PATH, env) loadScript(FILE_PATH .. "view", env)() loadScript(FILE_PATH .. "pilot", env)() loadScript(FILE_PATH .. "radar", env)() diff --git a/src/iNav/config.lua b/src/iNav/config.lua index a4701cb0..7800c3df 100644 --- a/src/iNav/config.lua +++ b/src/iNav/config.lua @@ -16,14 +16,14 @@ local config = { { o = 10, c = 1, v = 1 }, -- Altitude Alert - 12 { o = 12, c = 1, v = 1, x = 3 }, -- Timer - 13 { o = 14, c = 1, v = 1 }, -- Rx Voltage - 14 - { o = 33, c = 1, v = 0, x = 0 }, -- GPS - 15 - { o = 32, c = 1, v = 0 }, -- GPS Coordinates - 16 + { o = 28, c = 1, v = 0 }, -- Flight Path Vector - 15 + { o = 33, c = 1, v = 0 }, -- GPS - 16 { o = 9, c = 2, v = 20, x = 40 }, -- Fuel Critical - 17 { o = 8, c = 2, v = 30, x = 50 }, -- Fuel Low - 18 { o = 13, c = 1, v = SMLCD and 1 or 2, x = SMLCD and 1 or 2 }, -- Tx Voltage - 19 { o = 24, c = 1, v = 0 }, -- Speed Sensor - 20 - { o = 31, c = 2, v = 3.5, d = true, x = 5.0 }, -- GPS Warning - 21 - { o = 30, c = 1, v = 0 }, -- GPS HDOP View - 22 + { o = 32, c = 2, v = 3.5, d = true, x = 5.0 }, -- GPS Warning - 21 + { o = 31, c = 1, v = 0 }, -- GPS HDOP View - 22 { o = 6, c = 1, v = 0, x = 2 }, -- Fuel Unit - 23 { o = 16, c = 1, v = 3, x = 9 }, -- Vario Steps - 24 { o = 25, c = 1, v = 0, x = 3 }, -- View Mode - 25 @@ -32,9 +32,9 @@ local config = { { o = 17, c = 1, v = 0, x = 6 }, -- Altitude Graph - 28 { o = 5, c = 2, v = 4.3, d = true, x = 4.5 }, -- Cell Calculation - 29 { o = 27, c = 1, v = 0, x = 5 }, -- Aircraft Symbol - 30 - { o = 28, c = 1, v = 0 }, -- Center Map Home - 31 - { o = 29, c = 1, v = 0 }, -- Orientation - 32 - { o = 26, c = 1, v = 0 }, -- Roll Indicator - 33 + { o = 29, c = 1, v = 0 }, -- Center Map Home - 31 + { o = 30, c = 1, v = 0 }, -- Orientation - 32 + { o = 26, c = 1, v = 0 }, -- Roll Scale - 33 } for i = 1, #config do diff --git a/src/iNav/crsf.lua b/src/iNav/crsf.lua index a0e431c2..434131d1 100644 --- a/src/iNav/crsf.lua +++ b/src/iNav/crsf.lua @@ -1,13 +1,6 @@ local config, data, getTelemetryId = ... -local function getCrsfUnit(n) - local field = getFieldInfo(n) - return field and field.unit or 0 -end - data.crsf = true -data.rssi_id = getTelemetryId("RQly") -data.rssiMin_id = getTelemetryId("RQly-") data.rfmd_id = getTelemetryId("RFMD") data.sat_id = getTelemetryId("Sats") data.fuel_id = getTelemetryId("Capa") @@ -15,8 +8,10 @@ data.batt_id = getTelemetryId("RxBt") data.battMin_id = getTelemetryId("RxBt-") data.tpwr_id = getTelemetryId("TPWR") data.hdg_id = getTelemetryId("Yaw") -data.rssiMin = 100 +data.fpv_id = getTelemetryId("Hdg") data.tpwr = 0 +data.fpv = 0 +data.fuelRaw = 0 config[7].v = 0 config[9].v = 0 config[14].v = 0 @@ -27,8 +22,6 @@ config[23].x = 1 -- Testing Crossfire --[[ if data.simu then - data.rssi_id = getTelemetryId("RSSI") - data.rssiMin_id = getTelemetryId("RSSI-") data.sat_id = getTelemetryId("Tmp2") data.fuel_id = getTelemetryId("Fuel") data.batt_id = getTelemetryId("VFAS") @@ -42,16 +35,23 @@ end ]] local function crsf(data) + if data.rssi == 99 then + data.rssi = data.rssi + 1 + end data.tpwr = getValue(data.tpwr_id) data.pitch = math.deg(getValue(data.pitch_id)) * 10 data.roll = math.deg(getValue(data.roll_id)) * 10 - --data.heading = math.deg(getValue(data.hdg_id)) -- Crossfire Hdg seems to be based on GPS movement - -- The following is done due to an int rollover bug in the Crossfire protocol + -- The following shenanigans are requred due to int rollover bugs in the Crossfire protocol for yaw and hdg local tmp = getValue(data.hdg_id) if tmp < -0.27 then tmp = tmp + 0.27 end - data.heading = math.deg(tmp) + data.heading = (math.deg(tmp) + 360) % 360 + -- Flight path vector + if data.fpv_id > -1 then + data.fpv = ((getValue(data.fpv_id) < 0 and getValue(data.fpv_id) + 65.54 or getValue(data.fpv_id)) * 10 + 360) % 360 + end + data.fuelRaw = data.fuel if data.showFuel and config[23].v == 0 then if data.fuelEst == -1 and data.cell > 0 then if data.fuel < 25 and config[29].v - data.cell >= 0.2 then @@ -64,7 +64,8 @@ local function crsf(data) end data.fm = getValue(data.fm_id) data.modePrev = data.mode - data.satellites = data.satellites + (math.floor(math.min(data.satellites + 10, 25) * 0.36 + 0.5) * 100) + --Fake HDOP based on satellite lock count and assume GPS fix when there's at least 6 satellites + data.satellites = data.satellites + (math.floor(math.min(data.satellites + 10, 25) * 0.36 + 0.5) * 100) + (data.satellites >= 6 and 1000 or 0) -- In Betaflight 4.0+, flight mode ends with '*' when not armed local bfArmed = true @@ -111,8 +112,8 @@ local function crsf(data) -- Arming disabled data.mode = 2 else - -- Not in a waiting or error state so it must have a satellite lock and home position set - data.satellites = data.satellites + 3000 + -- Not in a waiting or error state so it must have a GPS home fix + data.satellites = data.satellites + 2000 -- Home reset, use last mode if data.fm == "HRST" then data.satellites = data.satellites + 4000 @@ -140,7 +141,7 @@ local function crsf(data) elseif data.fm == "WP" then data.mode = 2015 elseif data.fm == "RTH" then - data.mode = 1015 + data.mode = 1615 elseif data.fm == "!FS!" then data.mode = 40004 end diff --git a/src/iNav/data.lua b/src/iNav/data.lua index 1073e377..089002e4 100644 --- a/src/iNav/data.lua +++ b/src/iNav/data.lua @@ -48,8 +48,6 @@ local data = { a4_id = getTelemetryId("A4"), a4Min_id = getTelemetryId("A4-"), fuel_id = getTelemetryId("Fuel"), - rssi_id = getTelemetryId("RSSI"), - rssiMin_id = getTelemetryId("RSSI-"), vspeed_id = getTelemetryId("VSpd"), txBatt_id = getTelemetryId("tx-voltage"), gpsAlt_unit = getTelemetryUnit("GAlt"), @@ -74,6 +72,7 @@ local data = { rssiLast = 0, vspeed = 0, hdop = 0, + throttle = 0, homeResetPrev = false, gpsFixPrev = false, altNextPlay = 0, @@ -91,7 +90,7 @@ local data = { configSelect = 0, crsf = false, v = -1, - simu = string.sub(r, -4) == "simu" and true or false, + simu = string.sub(r, -4) == "simu", msg = m + i / 10 < 2.2 and "OpenTX v2.2+ Required" or false, lastLock = { lat = 0, lon = 0 }, fUnit = {"mAh", "mWh"}, diff --git a/src/iNav/func_h.lua b/src/iNav/func_h.lua index 5621444f..59f79411 100644 --- a/src/iNav/func_h.lua +++ b/src/iNav/func_h.lua @@ -8,43 +8,46 @@ if type(iNavZone) == "table" and type(iNavZone.zone) ~= "nil" then end local function title(data, config, SMLCD) - lcd.setColor(CUSTOM_COLOR, BLACK) - lcd.drawFilledRectangle(0, 0, LCD_W, 20, CUSTOM_COLOR) - lcd.drawText(0, 0, model.getInfo().name) + local text = lcd.drawText + local fill = lcd.drawFilledRectangle + local color = lcd.setColor + local fmt = string.format + + color(CUSTOM_COLOR, BLACK) + fill(0, 0, LCD_W, 20, CUSTOM_COLOR) + text(0, 0, model.getInfo().name) if config[13].v > 0 then lcd.drawTimer(340, 0, data.timer) end if config[19].v > 0 then - lcd.setColor(CUSTOM_COLOR, WHITE) - lcd.drawFilledRectangle(197, 3, 43, 14, CUSTOM_COLOR) - lcd.drawFilledRectangle(240, 6, 2, 8, CUSTOM_COLOR) + fill(197, 3, 43, 14, TEXT_COLOR) + fill(240, 6, 2, 8, TEXT_COLOR) local tmp = math.max(math.min((data.txBatt - data.txBattMin) / (data.txBattMax - data.txBattMin) * 42, 42), 0) + 197 - lcd.setColor(CUSTOM_COLOR, BLACK) for i = 200, tmp, 4 do - lcd.drawFilledRectangle(i, 5, 2, 10, CUSTOM_COLOR) + fill(i, 5, 2, 10, CUSTOM_COLOR) end end if config[19].v ~= 1 then - lcd.drawText(290, 0, string.format("%.1fV", data.txBatt), RIGHT) + text(290, 0, fmt("%.1fV", data.txBatt), RIGHT) end if data.rxBatt > 0 and data.telem and config[14].v == 1 then - lcd.drawText(LCD_W, 0, string.format("%.1fV", data.rxBatt), RIGHT) + text(LCD_W, 0, fmt("%.1fV", data.rxBatt), RIGHT) elseif data.crsf then - lcd.drawText(LCD_W, 0, (getValue(data.rfmd_id) == 2 and 150 or (data.telem and 50 or "--")) .. "Hz", RIGHT + (data.telem == false and WARNING_COLOR or 0)) + text(LCD_W, 0, (getValue(data.rfmd_id) == 2 and 150 or (data.telem and 50 or "--")) .. "Hz", RIGHT + (data.telem == false and WARNING_COLOR or 0)) end --[[ Show FPS data.frames = data.frames + 1 - lcd.drawText(180, 0, string.format("%.1f", data.frames / (getTime() - data.fpsStart) * 100), RIGHT) - --lcd.drawText(130, 0, string.format("%.1f", math.min(100 / (getTime() - data.start), 20)), RIGHT) + text(180, 0, fmt("%.1f", data.frames / (getTime() - data.fpsStart) * 100), RIGHT) + --text(130, 0, fmt("%.1f", math.min(100 / (getTime() - data.start), 20)), RIGHT) ]] -- Reset colors - lcd.setColor(WARNING_COLOR, YELLOW) + color(WARNING_COLOR, YELLOW) if data.widget then if iNavZone.options.Restore % 2 == 1 then - lcd.setColor(TEXT_COLOR, iNavZone.options.Text) - lcd.setColor(WARNING_COLOR, iNavZone.options.Warning) + color(TEXT_COLOR, iNavZone.options.Text) + color(WARNING_COLOR, iNavZone.options.Warning) end end end @@ -56,19 +59,20 @@ local function gpsDegMin(c, lat) end local function hdopGraph(x, y) + local fill = lcd.drawFilledRectangle lcd.setColor(CUSTOM_COLOR, data.hdop < 11 - config[21].v * 2 and YELLOW or WHITE) for i = 4, 9 do if i > data.hdop then lcd.setColor(CUSTOM_COLOR, GREY) end - lcd.drawRectangle(i * 4 + x - 16, y, 2, -i * 3 + 10, CUSTOM_COLOR) + fill(i * 4 + x - 16, y - (i * 3 - 10), 2, i * 3 - 10, CUSTOM_COLOR) end end local icons = {} - icons.lock = Bitmap.open(FILE_PATH .. "pics/lock.png") icons.home = Bitmap.open(FILE_PATH .. "pics/home.png") +icons.fpv = Bitmap.open(FILE_PATH .. "pics/fpv.png") icons.bg = Bitmap.open(FILE_PATH .. "pics/bg.png") icons.roll = Bitmap.open(FILE_PATH .. "pics/roll.png") icons.fg = Bitmap.open(FILE_PATH .. "pics/fg" .. config[30].v .. ".png") @@ -80,36 +84,54 @@ function icons.sym(fg) lcd.setColor(CUSTOM_COLOR, 25121) -- Ground lcd.drawFilledRectangle(356, 142, 123, 31, CUSTOM_COLOR) lcd.drawBitmap(fg, 355, 110, 50) - lcd.setColor(CUSTOM_COLOR, WHITE) - lcd.drawRectangle(355, 110, 125, 64, CUSTOM_COLOR) + lcd.drawRectangle(355, 110, 125, 64, TEXT_COLOR) end -if data.widget then - data.hcurx_id = getFieldInfo("ail").id - data.hcury_id = getFieldInfo("ele").id - data.hctrl_id = getFieldInfo("rud").id +data.hcurx_id = getFieldInfo("ail").id +data.hcury_id = getFieldInfo("ele").id +data.hctrl_id = getFieldInfo("rud").id +data.t6_id = getFieldInfo("trim-t6").id +data.lastevt = 0 +data.lastt6 = nil +function icons.alert() + lcd.setColor(CUSTOM_COLOR, BLACK) + lcd.drawFilledRectangle(20, 128, 439, 30, CUSTOM_COLOR) + lcd.setColor(CUSTOM_COLOR, YELLOW) + lcd.drawRectangle(19, 127, 441, 32, CUSTOM_COLOR) + lcd.drawText(28, 128, data.stickMsg, MIDSIZE + CUSTOM_COLOR) end function widgetEvt(data) local evt = 0 if not data.armed then - if data.throttle > 940 and getValue(data.hctrl_id) > 940 then - evt = EVT_SYS_FIRST - elseif getValue(data.hcurx_id) < -940 then - evt = EVT_EXIT_BREAK - elseif getValue(data.hcurx_id) > 940 then - evt = EVT_ENTER_BREAK - elseif getValue(data.hcury_id) > 200 then - evt = EVT_ROT_LEFT - elseif getValue(data.hcury_id) < -200 then - evt = EVT_ROT_RIGHT + data.stickMsg = (data.throttle >= -940 or math.abs(getValue(data.hctrl_id)) >= 50) and "Return throttle stick to bottom center" or nil + if data.throttle > 940 and getValue(data.hctrl_id) > 940 and math.abs(getValue(data.hcurx_id)) < 50 and math.abs(getValue(data.hcury_id)) < 50 then + evt = EVT_SYS_FIRST -- Enter config menu + elseif data.stickMsg == nil then + if getValue(data.hcurx_id) < -940 then + evt = EVT_EXIT_BREAK -- Left (exit) + elseif getValue(data.hcurx_id) > 940 then + evt = EVT_ENTER_BREAK -- Right (enter) + elseif getValue(data.hcury_id) > 200 then + evt = EVT_ROT_LEFT -- Up + elseif getValue(data.hcury_id) < -200 then + evt = EVT_ROT_RIGHT -- Down + end end + if data.lastevt == evt and (data.configStatus == 0 or math.abs(getValue(data.hcury_id)) < 940) then + evt = 0 + else + data.lastevt = evt + end + end + if evt == 0 and data.lastt6 ~= nil and getValue(data.t6_id) ~= data.lastt6 then + evt = EVT_ROT_RIGHT -- Down end - if data.lastevt == evt and math.abs(getValue(data.hcury_id)) < 940 then - evt = 0 - else - data.lastevt = evt + data.lastt6 = getValue(data.t6_id) + if data.lastt6 == 0 then + data.lastt6 = nil end + return evt end diff --git a/src/iNav/horus.lua b/src/iNav/horus.lua index d2450d3c..cf4a9f68 100644 --- a/src/iNav/horus.lua +++ b/src/iNav/horus.lua @@ -1,14 +1,14 @@ -local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcTrig, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) - - local SKY = 982 --lcd.RGB(0, 121, 180) - local GROUND = 25121 --lcd.RGB(98, 68, 8) - --local SKY2 = 8943 --lcd.RGB(32, 92, 122) - --local GROUND2 = 20996 --lcd.RGB(81, 65, 36) - --local MAP = 800 --lcd.RGB(0, 100, 0) - --local DKMAP = 544 --lcd.RGB(0, 70, 0) - local LIGHTMAP = 1184 --lcd.RGB(0, 150, 0) - --local DATA = 264 --lcd.RGB(0, 32, 65) - local DKGREY = 12678 --lcd.RGB(48, 48, 48) +local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcBearing, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) + + local SKY = 982 --rgb(0, 121, 180) + local GROUND = 25121 --rgb(98, 68, 8) + --local SKY2 = 8943 --rgb(32, 92, 122) + --local GROUND2 = 20996 --rgb(81, 65, 36) + --local MAP = 800 --rgb(0, 100, 0) + --local DKMAP = 544 --rgb(0, 70, 0) + local LIGHTMAP = 1184 --rgb(0, 150, 0) + --local DATA = 264 --rgb(0, 32, 65) + local DKGREY = 12678 --rgb(48, 48, 48) local RIGHT_POS = 270 local X_CNTR = 134 --(RIGHT_POS + LEFT_POS [0]) / 2 - 1 local HEADING_DEG = 190 @@ -16,7 +16,22 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic local TOP = 20 local BOTTOM = 146 local Y_CNTR = 83 --(TOP + BOTTOM) / 2 - local tmp, top2, bot2, pitch, roll, roll1, upsideDown + local tmp, tmp2, top2, bot2, pitch, roll, roll1, upsideDown + local text = lcd.drawText + local line = lcd.drawLine + local fill = lcd.drawFilledRectangle + local bmap = lcd.drawBitmap + local rgb = lcd.RGB + local color = lcd.setColor + local max = math.max + local min = math.min + local floor = math.floor + local abs = math.abs + local rad = math.rad + local deg = math.deg + local sin = math.sin + local cos = math.cos + local fmt = string.format function intersect(s1, e1, s2, e2) local d = (s1.x - e1.x) * (s2.y - e2.y) - (s1.y - e1.y) * (s2.x - e2.x) @@ -24,66 +39,66 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic local b = s2.x * e2.y - s2.y * e2.x local x = (a * (s2.x - e2.x) - (s1.x - e1.x) * b) / d local y = (a * (s2.y - e2.y) - (s1.y - e1.y) * b) / d - if x < math.min(s2.x, e2.x) - 1 or x > math.max(s2.x, e2.x) + 1 or y < math.min(s2.y, e2.y) - 1 or y > math.max(s2.y, e2.y) + 1 then + if x < min(s2.x, e2.x) - 1 or x > max(s2.x, e2.x) + 1 or y < min(s2.y, e2.y) - 1 or y > max(s2.y, e2.y) + 1 then return nil, nil end - return math.floor(x + 0.5), math.floor(y + 0.5) + return floor(x + 0.5), floor(y + 0.5) end local function pitchLadder(r, adj) --[[ Caged mode - local x = math.sin(roll1) * r - local y = math.cos(roll1) * r - local p = math.cos(math.rad(pitch - adj)) * 170 + local x = sin(roll1) * r + local y = cos(roll1) * r + local p = cos(rad(pitch - adj)) * 170 local x1, y1, x2, y2 = X_CNTR - x, Y_CNTR + y - p, X_CNTR + x, Y_CNTR - y - p ]] -- Uncaged mode - local p = math.sin(math.rad(adj)) * 170 - local y = (Y_CNTR - math.cos(math.rad(pitch)) * 170) - math.sin(roll1) * p + local p = sin(rad(adj)) * 170 + local y = (Y_CNTR - cos(rad(pitch)) * 170) - sin(roll1) * p if y > top2 and y < bot2 then - local x = X_CNTR - math.cos(roll1) * p - local xd = math.sin(roll1) * r - local yd = math.cos(roll1) * r + local x = X_CNTR - cos(roll1) * p + local xd = sin(roll1) * r + local yd = cos(roll1) * r local x1, y1, x2, y2 = x - xd, y + yd, x + xd, y - yd - if (y1 > top2 or y2 > top2) and (y1 < bot2 or y2 < bot2) and y1 >= 0 and y2 >= 0 then - lcd.setColor(CUSTOM_COLOR, r == 20 and WHITE or LIGHTGREY) - lcd.drawLine(x1, y1, x2, y2, SOLID, CUSTOM_COLOR) + if (y1 > top2 or y2 > top2) and (y1 < bot2 or y2 < bot2) and x1 >= 0 and x2 >= 0 then + color(CUSTOM_COLOR, r == 20 and WHITE or LIGHTGREY) + line(x1, y1, x2, y2, SOLID, CUSTOM_COLOR) if r == 20 and y1 > top2 and y1 < bot2 then - lcd.drawText(x1 - 1, y1 - 8, upsideDown and -adj or adj, SMLSIZE + RIGHT) + text(x1 - 1, y1 - 8, upsideDown and -adj or adj, SMLSIZE + RIGHT) end end end end local function tics(v, p) - tmp = math.floor((v + 25) / 10) * 10 + tmp = floor((v + 25) / 10) * 10 for i = tmp - 40, tmp, 5 do local tmp2 = Y_CNTR + ((v - i) * 3) - 9 if tmp2 > 10 and tmp2 < BOTTOM - 8 then - lcd.drawLine(p, tmp2 + 8, p + 2, tmp2 + 8, SOLID, TEXT_COLOR) + line(p, tmp2 + 8, p + 2, tmp2 + 8, SOLID, TEXT_COLOR) if config[28].v == 0 and i % 10 == 0 and (i >= 0 or p > X_CNTR) and tmp2 < BOTTOM - 23 then - lcd.drawText(p + (p > X_CNTR and -1 or 4), tmp2, i, SMLSIZE + (p > X_CNTR and RIGHT or 0) + TEXT_COLOR) + text(p + (p > X_CNTR and -1 or 4), tmp2, i, SMLSIZE + (p > X_CNTR and RIGHT or 0) + TEXT_COLOR) end end end end -- Setup - lcd.drawBitmap(icons.bg, 0, TOP) - lcd.setColor(TEXT_COLOR, WHITE) - lcd.setColor(WARNING_COLOR, data.telem and YELLOW or RED) + bmap(icons.bg, 0, TOP) + color(TEXT_COLOR, WHITE) + color(WARNING_COLOR, data.telem and YELLOW or RED) -- Calculate orientation if data.pitchRoll then - pitch = (math.abs(data.roll) > 900 and -1 or 1) * (270 - data.pitch / 10) % 180 + pitch = (abs(data.roll) > 900 and -1 or 1) * (270 - data.pitch / 10) % 180 roll = (270 - data.roll / 10) % 180 - upsideDown = math.abs(data.roll) > 900 + upsideDown = abs(data.roll) > 900 else - pitch = 90 - math.deg(math.atan2(data.accx * (data.accz >= 0 and -1 or 1), math.sqrt(data.accy * data.accy + data.accz * data.accz))) - roll = 90 - math.deg(math.atan2(data.accy * (data.accz >= 0 and 1 or -1), math.sqrt(data.accx * data.accx + data.accz * data.accz))) + pitch = 90 - deg(math.atan2(data.accx * (data.accz >= 0 and -1 or 1), math.sqrt(data.accy * data.accy + data.accz * data.accz))) + roll = 90 - deg(math.atan2(data.accy * (data.accz >= 0 and 1 or -1), math.sqrt(data.accx * data.accx + data.accz * data.accz))) upsideDown = data.accz < 0 end - roll1 = math.rad(roll) + roll1 = rad(roll) top2 = config[33].v == 0 and TOP or TOP + 20 bot2 = BOTTOM - 15 local i = { {}, {} } @@ -94,9 +109,9 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic local skip = false -- Calculate horizon (uses simple "caged" mode for less math) - local x = math.sin(roll1) * 200 - local y = math.cos(roll1) * 200 - local p = math.cos(math.rad(pitch)) * 170 + local x = sin(roll1) * 200 + local y = cos(roll1) * 200 + local p = cos(rad(pitch)) * 170 local h1 = { x = X_CNTR + x, y = Y_CNTR - y - p } local h2 = { x = X_CNTR - x, y = Y_CNTR + y - p } @@ -121,42 +136,42 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic end -- Draw ground - lcd.setColor(CUSTOM_COLOR, GROUND) + color(CUSTOM_COLOR, GROUND) if skip then -- Must be going down hard! if (pitch - 90) * (upsideDown and -1 or 1) < 0 then - lcd.drawFilledRectangle(tl.x, tl.y, br.x - tl.x + 1, br.y - tl.y + 1, CUSTOM_COLOR) + fill(tl.x, tl.y, br.x - tl.x + 1, br.y - tl.y + 1, CUSTOM_COLOR) end else local trix, triy -- Find right angle coordinates of triangle if upsideDown then - trix = roll > 90 and math.max(i[1].x, i[2].x) or math.min(i[1].x, i[2].x) - triy = math.min(i[1].y, i[2].y) + trix = roll > 90 and max(i[1].x, i[2].x) or min(i[1].x, i[2].x) + triy = min(i[1].y, i[2].y) else - trix = roll > 90 and math.min(i[1].x, i[2].x) or math.max(i[1].x, i[2].x) - triy = math.max(i[1].y, i[2].y) + trix = roll > 90 and min(i[1].x, i[2].x) or max(i[1].x, i[2].x) + triy = max(i[1].y, i[2].y) end -- Find rectangle(s) and fill if upsideDown then if triy > tl.y then - lcd.drawFilledRectangle(tl.x, tl.y, br.x - tl.x + 1, triy - tl.y, CUSTOM_COLOR) + fill(tl.x, tl.y, br.x - tl.x + 1, triy - tl.y, CUSTOM_COLOR) end if roll > 90 and trix < br.x then - lcd.drawFilledRectangle(trix, triy, br.x - trix + 1, br.y - triy + 1, CUSTOM_COLOR) + fill(trix, triy, br.x - trix + 1, br.y - triy + 1, CUSTOM_COLOR) elseif roll <= 90 and trix > tl.x then - lcd.drawFilledRectangle(tl.x, triy, trix - tl.x, br.y - triy + 1, CUSTOM_COLOR) + fill(tl.x, triy, trix - tl.x, br.y - triy + 1, CUSTOM_COLOR) end else if triy < br.y then - lcd.drawFilledRectangle(tl.x, triy + 1, br.x - tl.x + 1, br.y - triy, CUSTOM_COLOR) + fill(tl.x, triy + 1, br.x - tl.x + 1, br.y - triy, CUSTOM_COLOR) end if roll > 90 and trix > tl.x then - lcd.drawFilledRectangle(tl.x, tl.y, trix - tl.x, triy - tl.y + 1, CUSTOM_COLOR) + fill(tl.x, tl.y, trix - tl.x, triy - tl.y + 1, CUSTOM_COLOR) elseif roll <= 90 and trix < br.x then - lcd.drawFilledRectangle(trix, tl.y, br.x - trix + 1, triy - tl.y + 1, CUSTOM_COLOR) + fill(trix, tl.y, br.x - trix + 1, triy - tl.y + 1, CUSTOM_COLOR) end end @@ -169,29 +184,29 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic end local inc = 1 if height ~= 0 then - local width = math.abs(i[1].x - trix) + local width = abs(i[1].x - trix) local tx1 = i[1].x local tx2 = trix if width == 0 then - width = math.abs(i[2].x - trix) + width = abs(i[2].x - trix) tx1 = i[2].x tx2 = trix end - inc = math.abs(height) < 10 and 1 or (math.abs(height) < 20 and 2 or ((math.abs(height) < width and math.abs(roll - 90) < 55) and 3 or 5)) + inc = abs(height) < 10 and 1 or (abs(height) < 20 and 2 or ((abs(height) < width and abs(roll - 90) < 55) and 3 or 5)) local steps = height > 0 and inc or -inc local slope = width / height * inc local s = slope > 0 and 0 or inc - 1 - slope = math.abs(slope) * (tx1 < tx2 and 1 or -1) + slope = abs(slope) * (tx1 < tx2 and 1 or -1) for y = triy, top, steps do - if math.abs(steps) == 1 then - lcd.drawLine(tx1, y, tx2, y, SOLID, CUSTOM_COLOR) + if abs(steps) == 1 then + line(tx1, y, tx2, y, SOLID, CUSTOM_COLOR) else if tx1 < tx2 then --if tx1 < tx2 and tx2 - tx1 + 1 > 0 then - lcd.drawFilledRectangle(tx1, y - s, tx2 - tx1 + 1, inc, CUSTOM_COLOR) + fill(tx1, y - s, tx2 - tx1 + 1, inc, CUSTOM_COLOR) else --elseif tx1 > tx2 and tx1 - tx2 + 1 > 0 then - lcd.drawFilledRectangle(tx2, y - s, tx1 - tx2 + 1, inc, CUSTOM_COLOR) + fill(tx2, y - s, tx1 - tx2 + 1, inc, CUSTOM_COLOR) end end tx1 = tx1 + slope @@ -202,63 +217,48 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic if not upsideDown and inc <= 3 then if inc > 1 then if inc > 2 then - lcd.drawLine(i[1].x, i[1].y + 2, i[2].x, i[2].y + 2, SOLID, CUSTOM_COLOR) + line(i[1].x, i[1].y + 2, i[2].x, i[2].y + 2, SOLID, CUSTOM_COLOR) end - lcd.drawLine(i[1].x, i[1].y + 1, i[2].x, i[2].y + 1, SOLID, CUSTOM_COLOR) - lcd.setColor(CUSTOM_COLOR, SKY) - lcd.drawLine(i[1].x, i[1].y - 1, i[2].x, i[2].y - 1, SOLID, CUSTOM_COLOR) + line(i[1].x, i[1].y + 1, i[2].x, i[2].y + 1, SOLID, CUSTOM_COLOR) + color(CUSTOM_COLOR, SKY) + line(i[1].x, i[1].y - 1, i[2].x, i[2].y - 1, SOLID, CUSTOM_COLOR) if inc > 2 then - lcd.drawLine(i[1].x, i[1].y - 2, i[2].x, i[2].y - 2, SOLID, CUSTOM_COLOR) + line(i[1].x, i[1].y - 2, i[2].x, i[2].y - 2, SOLID, CUSTOM_COLOR) end if 90 - roll > 25 then - lcd.drawLine(i[1].x, i[1].y - 3, i[2].x, i[2].y - 3, SOLID, CUSTOM_COLOR) + line(i[1].x, i[1].y - 3, i[2].x, i[2].y - 3, SOLID, CUSTOM_COLOR) end end - lcd.setColor(CUSTOM_COLOR, LIGHTGREY) - lcd.drawLine(i[1].x, i[1].y, i[2].x, i[2].y, SOLID, CUSTOM_COLOR) + color(CUSTOM_COLOR, LIGHTGREY) + line(i[1].x, i[1].y, i[2].x, i[2].y, SOLID, CUSTOM_COLOR) end end -- Pitch ladder if data.telem then tmp = pitch - 90 - local tmp2 = math.max(math.min((tmp >= 0 and math.floor(tmp / 5) or math.ceil(tmp / 5)) * 5, 30), -30) + local tmp2 = max(min((tmp >= 0 and floor(tmp / 5) or math.ceil(tmp / 5)) * 5, 30), -30) for x = tmp2 - 20, tmp2 + 20, 5 do if x ~= 0 and (x % 10 == 0 or (x > -30 and x < 30)) then pitchLadder(x % 10 == 0 and 20 or 15, x) end end if not data.showMax then - --[[ Adds a shadow to the pitch - lcd.setColor(CUSTOM_COLOR, BLACK) - lcd.drawText(X_CNTR - 64, Y_CNTR - 8, string.format("%.0f", upsideDown and -tmp or tmp) .. "\64", SMLSIZE + RIGHT + CUSTOM_COLOR) - ]] - lcd.drawText(X_CNTR - 65, Y_CNTR - 9, string.format("%.0f", upsideDown and -tmp or tmp) .. "\64", SMLSIZE + RIGHT) - end - end - - -- Home direction - if data.showHead and data.armed and data.telem and data.gpsHome ~= false and data.startup == 0 then - if data.distanceLast >= data.distRef then - local bearing = calcTrig(data.gpsHome, data.gpsLatLon, true) + 540 % 360 - local home = math.floor(((bearing - data.heading + (361 + HEADING_DEG / 2)) % 360) * PIXEL_DEG - 2.5) - if home >= 3 and home <= RIGHT_POS - 6 then - lcd.drawBitmap(icons.home, home - 3, BOTTOM - 26) - end + text(X_CNTR - 65, Y_CNTR - 9, fmt("%.0f", upsideDown and -tmp or tmp) .. "\64", SMLSIZE + RIGHT) end end -- Compass if data.showHead then for i = 0, 348.75, 11.25 do - tmp = math.floor(((i - data.heading + (361 + HEADING_DEG / 2)) % 360) * PIXEL_DEG - 2.5) + tmp = floor(((i - data.heading + (361 + HEADING_DEG / 2)) % 360) * PIXEL_DEG - 2.5) if tmp >= 9 and tmp <= RIGHT_POS - 12 then if i % 90 == 0 then - lcd.drawText(tmp - (i < 270 and 3 or 5), bot2, i == 0 and "N" or (i == 90 and "E" or (i == 180 and "S" or "W")), SMLSIZE) + text(tmp - (i < 270 and 3 or 5), bot2, i == 0 and "N" or (i == 90 and "E" or (i == 180 and "S" or "W")), SMLSIZE) elseif i % 45 == 0 then - lcd.drawText(tmp - (i < 225 and 7 or 9), bot2, i == 45 and "NE" or (i == 135 and "SE" or (i == 225 and "SW" or "NW")), SMLSIZE) + text(tmp - (i < 225 and 7 or 9), bot2, i == 45 and "NE" or (i == 135 and "SE" or (i == 225 and "SW" or "NW")), SMLSIZE) else - lcd.drawLine(tmp, BOTTOM - 4, tmp, BOTTOM - 1, SOLID, 0) + line(tmp, BOTTOM - 4, tmp, BOTTOM - 1, SOLID, 0) end end end @@ -268,118 +268,145 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic tics(data.speed, 1) tics(data.altitude, RIGHT_POS - 4) if config[28].v == 0 and config[33].v == 0 then - lcd.drawText(42, TOP - 1, units[data.speed_unit], SMLSIZE) - lcd.drawText(RIGHT_POS - 45, TOP - 1, "Alt " .. units[data.alt_unit], SMLSIZE + RIGHT) + text(42, TOP - 1, units[data.speed_unit], SMLSIZE) + text(RIGHT_POS - 45, TOP - 1, "Alt " .. units[data.alt_unit], SMLSIZE + RIGHT) + elseif config[28].v > 0 then + text(39, Y_CNTR - 25, units[data.speed_unit], SMLSIZE + RIGHT) + text(RIGHT_POS - 6, Y_CNTR - 25, "Alt " .. units[data.alt_unit], SMLSIZE + RIGHT) end -- View overlay - lcd.drawBitmap(icons.fg, 1, 20) + bmap(icons.fg, 1, 20) -- Speed & altitude tmp = data.showMax and data.speedMax or data.speed - lcd.drawText(39, Y_CNTR - 9, tmp >= 99.5 and math.floor(tmp + 0.5) or string.format("%.1f", tmp), SMLSIZE + RIGHT + data.telemFlags) + text(39, Y_CNTR - 9, tmp >= 99.5 and floor(tmp + 0.5) or fmt("%.1f", tmp), SMLSIZE + RIGHT + data.telemFlags) tmp = data.showMax and data.altitudeMax or data.altitude - lcd.drawText(RIGHT_POS - 2, Y_CNTR - 9, math.floor(tmp + 0.5), SMLSIZE + RIGHT + ((not data.telem or tmp + 0.5 >= config[6].v) and FLASH or 0)) + text(RIGHT_POS - 2, Y_CNTR - 9, floor(tmp + 0.5), SMLSIZE + RIGHT + ((not data.telem or tmp + 0.5 >= config[6].v) and FLASH or 0)) if data.altHold then - lcd.drawBitmap(icons.lock, RIGHT_POS - 55, Y_CNTR - 5) + bmap(icons.lock, RIGHT_POS - 55, Y_CNTR - 5) end -- Heading if data.showHead then - lcd.drawText(X_CNTR + 18, bot2, math.floor(data.heading + 0.5) % 360 .. "\64", SMLSIZE + RIGHT + data.telemFlags) + text(X_CNTR + 18, bot2, floor(data.heading + 0.5) % 360 .. "\64", SMLSIZE + RIGHT + data.telemFlags) end - -- Roll indicator + -- Roll scale if config[33].v == 1 then - lcd.drawBitmap(icons.roll, 43, 20) + bmap(icons.roll, 43, 20) if roll > 30 and roll < 150 and not upsideDown then - local x1, y1, x2, y2, x3, y3 = calcDir(math.rad(roll - 90), math.rad(roll + 55), math.rad(roll - 235), X_CNTR - (math.cos(roll1) * 75), 79 - (math.sin(roll1) * 40), 7) - lcd.setColor(CUSTOM_COLOR, YELLOW) - lcd.drawLine(x1, y1, x2, y2, SOLID, CUSTOM_COLOR) - lcd.drawLine(x1, y1, x3, y3, SOLID, CUSTOM_COLOR) - lcd.drawLine(x2, y2, x3, y3, SOLID, CUSTOM_COLOR) + local x1, y1, x2, y2, x3, y3 = calcDir(rad(roll - 90), rad(roll + 55), rad(roll - 235), X_CNTR - (cos(roll1) * 75), 79 - (sin(roll1) * 40), 7) + color(CUSTOM_COLOR, YELLOW) + line(x1, y1, x2, y2, SOLID, CUSTOM_COLOR) + line(x1, y1, x3, y3, SOLID, CUSTOM_COLOR) + line(x2, y2, x3, y3, SOLID, CUSTOM_COLOR) + end + end + + -- Home direction + if data.showHead and data.armed and data.telem and data.gpsHome ~= false and data.startup == 0 then + if data.distanceLast >= data.distRef then + local bearing = calcBearing(data.gpsHome, data.gpsLatLon) + 540 % 360 + local home = floor(((bearing - data.heading + (361 + HEADING_DEG / 2)) % 360) * PIXEL_DEG - 2.5) + if home >= 3 and home <= RIGHT_POS - 6 then + bmap(icons.home, home - 3, BOTTOM - 26) + end + end + -- Flight path vector + if data.crsf and data.fpv_id > -1 and config[15].v == 1 and data.speed >= 8 then + tmp = (data.fpv - data.heading + 360) % 360 + if tmp >= 302 or tmp <= 57 then + local fpv = floor(((data.fpv - data.heading + (361 + HEADING_DEG / 2)) % 360) * PIXEL_DEG - 0.5) + local r = fpv - X_CNTR -- Adjust from center + local adj = pitch - 90 -- Pitch degrees, change to climb/descend vector + local p = sin(rad(adj)) * 170 + local x = (X_CNTR - cos(roll1) * p) + (sin(roll1) * r) - 9 + local y = ((Y_CNTR - cos(rad(pitch)) * 170) - sin(roll1) * p) - (cos(roll1) * r) - 6 + if y > top2 and y < bot2 and x >= 0 then + bmap(icons.fpv, x, y) + end + end end end -- Variometer if config[7].v % 2 == 1 then - lcd.setColor(CUSTOM_COLOR, DKGREY) - lcd.drawFilledRectangle(RIGHT_POS, TOP, 10, BOTTOM - 20, CUSTOM_COLOR) - lcd.setColor(CUSTOM_COLOR, LIGHTGREY) - lcd.drawLine(RIGHT_POS + 10, TOP, RIGHT_POS + 10, BOTTOM - 1, SOLID, CUSTOM_COLOR) - lcd.setColor(CUSTOM_COLOR, GREY) - lcd.drawLine(RIGHT_POS, Y_CNTR - 1, RIGHT_POS + 9, Y_CNTR - 1, SOLID, CUSTOM_COLOR) + color(CUSTOM_COLOR, DKGREY) + fill(RIGHT_POS, TOP, 10, BOTTOM - 20, CUSTOM_COLOR) + color(CUSTOM_COLOR, LIGHTGREY) + line(RIGHT_POS + 10, TOP, RIGHT_POS + 10, BOTTOM - 1, SOLID, CUSTOM_COLOR) + color(CUSTOM_COLOR, GREY) + line(RIGHT_POS, Y_CNTR - 1, RIGHT_POS + 9, Y_CNTR - 1, SOLID, CUSTOM_COLOR) if data.telem then - lcd.setColor(CUSTOM_COLOR, YELLOW) - tmp = math.log(1 + math.min(math.abs(0.6 * (data.vspeed_unit == 6 and data.vspeed / 3.28084 or data.vspeed)), 10)) * (data.vspeed < 0 and -1 or 1) + color(CUSTOM_COLOR, YELLOW) + tmp = math.log(1 + min(abs(0.6 * (data.vspeed_unit == 6 and data.vspeed / 3.28084 or data.vspeed)), 10)) * (data.vspeed < 0 and -1 or 1) local y1 = Y_CNTR - (tmp / 2.4 * (Y_CNTR - 21)) local y2 = Y_CNTR - (tmp / 2.6 * (Y_CNTR - 21)) - lcd.drawLine(RIGHT_POS, y1 - 1, RIGHT_POS + 9, y2 - 1, SOLID, CUSTOM_COLOR) - lcd.drawLine(RIGHT_POS, y1, RIGHT_POS + 9, y2, SOLID, CUSTOM_COLOR) + line(RIGHT_POS, y1 - 1, RIGHT_POS + 9, y2 - 1, SOLID, CUSTOM_COLOR) + line(RIGHT_POS, y1, RIGHT_POS + 9, y2, SOLID, CUSTOM_COLOR) end if data.startup == 0 then - lcd.drawText(RIGHT_POS + 13, TOP - 1, string.format(math.abs(data.vspeed) >= 9.95 and "%.0f" or "%.1f", data.vspeed) .. units[data.vspeed_unit], SMLSIZE + data.telemFlags) + text(RIGHT_POS + 13, TOP - 1, fmt(abs(data.vspeed) >= 9.95 and "%.0f" or "%.1f", data.vspeed) .. units[data.vspeed_unit], SMLSIZE + data.telemFlags) end end -- Calc orientation tmp = data.headingRef - if data.showDir or data.headingRef < 0 then + if data.showDir or data.headingRef == -1 then tmp = 0 end - local r1 = math.rad(data.heading - tmp) - local r2 = math.rad(data.heading - tmp + 145) - local r3 = math.rad(data.heading - tmp - 145) + local r1 = rad(data.heading - tmp) + local r2 = rad(data.heading - tmp + 145) + local r3 = rad(data.heading - tmp - 145) -- Radar local LEFT_POS = RIGHT_POS + (config[7].v % 2 == 1 and 11 or 0) RIGHT_POS = 479 X_CNTR = (RIGHT_POS + LEFT_POS) / 2 - 1 - local tmp2 = data.showMax and data.distanceMax or data.distanceLast - local dist = tmp2 < 1000 and math.floor(tmp2 + 0.5) .. units[data.dist_unit] or (string.format("%.1f", tmp2 / (data.dist_unit == 9 and 1000 or 5280)) .. (data.dist_unit == 9 and "km" or "mi")) if data.startup == 0 then -- Launch/north-based orientation - if data.showDir or data.headingRef < 0 then - lcd.drawText(LEFT_POS + 2, Y_CNTR - 9, "W", SMLSIZE) - lcd.drawText(RIGHT_POS, Y_CNTR - 9, "E", SMLSIZE + RIGHT) + if data.showDir or data.headingRef == -1 then + text(LEFT_POS + 2, Y_CNTR - 9, "W", SMLSIZE) + text(RIGHT_POS, Y_CNTR - 9, "E", SMLSIZE + RIGHT) end local cx, cy, d -- Altitude graph if config[28].v > 0 then local factor = 30 / (data.altMax - data.altMin) - lcd.setColor(CUSTOM_COLOR, LIGHTMAP) + color(CUSTOM_COLOR, LIGHTMAP) for i = 1, 60 do cx = RIGHT_POS - 60 + i - cy = math.floor(BOTTOM - (data.alt[((data.altCur - 2 + i) % 60) + 1] - data.altMin) * factor + 0.5) + cy = floor(BOTTOM - (data.alt[((data.altCur - 2 + i) % 60) + 1] - data.altMin) * factor + 0.5) if cy < BOTTOM then - lcd.drawLine(cx, cy, cx, BOTTOM - 1, SOLID, CUSTOM_COLOR) + line(cx, cy, cx, BOTTOM - 1, SOLID, CUSTOM_COLOR) end if (i - 1) % (60 / config[28].v) == 0 then - lcd.setColor(CUSTOM_COLOR, DKGREY) - lcd.drawLine(cx, BOTTOM - 30, cx, BOTTOM - 1, DOTTED, CUSTOM_COLOR) - lcd.setColor(CUSTOM_COLOR, LIGHTMAP) + color(CUSTOM_COLOR, DKGREY) + line(cx, BOTTOM - 30, cx, BOTTOM - 1, DOTTED, CUSTOM_COLOR) + color(CUSTOM_COLOR, LIGHTMAP) end end if data.altMin < -1 then cy = BOTTOM - (-data.altMin * factor) - lcd.setColor(CUSTOM_COLOR, LIGHTGREY) - lcd.drawLine(RIGHT_POS - 58, cy, RIGHT_POS - 1, cy, DOTTED, CUSTOM_COLOR) + color(CUSTOM_COLOR, LIGHTGREY) + line(RIGHT_POS - 58, cy, RIGHT_POS - 1, cy, DOTTED, CUSTOM_COLOR) if cy < 142 then - lcd.drawText(RIGHT_POS - 59, cy - 8, "0", SMLSIZE + RIGHT) + text(RIGHT_POS - 59, cy - 8, "0", SMLSIZE + RIGHT) end end - lcd.drawText(RIGHT_POS + 2, BOTTOM - 46, math.floor(data.altMax + 0.5) .. units[data.alt_unit], SMLSIZE + RIGHT) + text(RIGHT_POS + 2, BOTTOM - 46, floor(data.altMax + 0.5) .. units[data.alt_unit], SMLSIZE + RIGHT) end if data.gpsHome ~= false then -- Craft location tmp2 = config[31].v == 1 and 50 or 100 - d = data.distanceLast >= data.distRef and math.min(math.max((data.distanceLast / math.max(math.min(data.distanceMax, data.distanceLast * 4), data.distRef * 10)) * tmp2, 7), tmp2) or 1 - local bearing = calcTrig(data.gpsHome, data.gpsLatLon, true) - tmp - local rad1 = math.rad(bearing) - cx = math.floor(math.sin(rad1) * d + 0.5) - cy = math.floor(math.cos(rad1) * d + 0.5) + d = data.distanceLast >= data.distRef and min(max((data.distanceLast / max(min(data.distanceMax, data.distanceLast * 4), data.distRef * 10)) * tmp2, 7), tmp2) or 1 + local bearing = calcBearing(data.gpsHome, data.gpsLatLon) - tmp + local rad1 = rad(bearing) + cx = floor(sin(rad1) * d + 0.5) + cy = floor(cos(rad1) * d + 0.5) -- Home position local hx = X_CNTR + 2 local hy = Y_CNTR @@ -388,9 +415,9 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic hy = hy + (d > 9 and cy / 2 or 0) end if d >= 12 then - lcd.drawBitmap(icons.home, hx - 4, hy - 5) + bmap(icons.home, hx - 4, hy - 5) elseif d > 1 then - lcd.drawFilledRectangle(hx - 1, hy - 1, 3, 3, SOLID) + fill(hx - 1, hy - 1, 3, 3, SOLID) end -- Shift craft location cx = d == 1 and X_CNTR + 2 or cx + hx @@ -402,20 +429,21 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic end -- Orientation local x1, y1, x2, y2, x3, y3 = calcDir(r1, r2, r3, cx, cy, 8) - lcd.setColor(CUSTOM_COLOR, LIGHTGREY) - lcd.drawLine(x2, y2, x3, y3, SOLID, CUSTOM_COLOR) - lcd.drawLine(x1, y1, x2, y2, SOLID, TEXT_COLOR) - lcd.drawLine(x1, y1, x3, y3, SOLID, TEXT_COLOR) - lcd.drawText(LEFT_POS + 2, BOTTOM - 16, dist, SMLSIZE + data.telemFlags) + color(CUSTOM_COLOR, LIGHTGREY) + line(x2, y2, x3, y3, SOLID, CUSTOM_COLOR) + line(x1, y1, x2, y2, SOLID, TEXT_COLOR) + line(x1, y1, x3, y3, SOLID, TEXT_COLOR) + tmp = data.distanceLast < 1000 and floor(data.distanceLast + 0.5) .. units[data.dist_unit] or (fmt("%.1f", data.distanceLast / (data.dist_unit == 9 and 1000 or 5280)) .. (data.dist_unit == 9 and "km" or "mi")) + text(LEFT_POS + 2, BOTTOM - 16, tmp, SMLSIZE + data.telemFlags) end -- Startup message if data.startup == 2 then - lcd.setColor(CUSTOM_COLOR, BLACK) - lcd.drawText(X_CNTR - 78, 55, "Lua Telemetry", MIDSIZE + CUSTOM_COLOR) - lcd.drawText(X_CNTR - 38, 85, "v" .. VERSION, MIDSIZE + CUSTOM_COLOR) - lcd.drawText(X_CNTR - 79, 54, "Lua Telemetry", MIDSIZE) - lcd.drawText(X_CNTR - 39, 84, "v" .. VERSION, MIDSIZE) + color(CUSTOM_COLOR, BLACK) + text(X_CNTR - 78, 55, "Lua Telemetry", MIDSIZE + CUSTOM_COLOR) + text(X_CNTR - 38, 85, "v" .. VERSION, MIDSIZE + CUSTOM_COLOR) + text(X_CNTR - 79, 54, "Lua Telemetry", MIDSIZE) + text(X_CNTR - 39, 84, "v" .. VERSION, MIDSIZE) end -- Data @@ -428,113 +456,127 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic -- Box 1 (fuel, battery, rssi) tmp = (not data.telem or data.cell < config[3].v or (data.showFuel and config[23].v == 0 and data.fuel <= config[17].v)) and FLASH or 0 if data.showFuel then - if config[23].v == 0 then - lcd.drawText(X1 - 3, TOP, data.fuel .. "%", MIDSIZE + RIGHT + tmp) - local red = data.fuel >= config[18].v and math.max(math.floor((100 - data.fuel) / (100 - config[18].v) * 255), 0) or 255 - local green = data.fuel < config[18].v and math.max(math.floor((data.fuel - config[17].v) / (config[18].v - config[17].v) * 255), 0) or 255 - lcd.setColor(CUSTOM_COLOR, lcd.RGB(red, green, 60)) - lcd.drawGauge(0, TOP + 26, X1 - 3, 15, math.min(data.fuel, 99), 100, CUSTOM_COLOR) + if config[23].v > 0 or (data.crsf and data.showMax) then + text(X1, TOP + 1, (data.crsf and data.fuelRaw or data.fuel) .. data.fUnit[data.crsf and 1 or config[23].v], MIDSIZE + RIGHT + tmp) else - lcd.drawText(X1, TOP + 1, data.fuel .. data.fUnit[config[23].v], MIDSIZE + RIGHT + tmp) + text(X1 - 3, TOP, data.fuel .. "%", MIDSIZE + RIGHT + tmp) + if data.fl ~= data.fuel then + local red = data.fuel >= config[18].v and max(floor((100 - data.fuel) / (100 - config[18].v) * 255), 0) or 255 + local green = data.fuel < config[18].v and max(floor((data.fuel - config[17].v) / (config[18].v - config[17].v) * 255), 0) or 255 + data.fc = rgb(red, green, 60) + data.fl = data.fuel + end + color(CUSTOM_COLOR, data.fc) + lcd.drawGauge(0, TOP + 26, X1 - 3, 15, min(data.fuel, 99), 100, CUSTOM_COLOR) end - lcd.drawText(0, TOP + (config[23].v == 0 and 9 or 23), labels[1], SMLSIZE) + text(0, TOP + ((config[23].v > 0 or (data.crsf and data.showMax)) and 23 or 9), labels[1], SMLSIZE) end - local val = data.showMax and data.cellMin or data.cell - lcd.drawText(X1 - 3, TOP + 42, string.format(config[1].v == 0 and "%.2fV" or "%.1fV", config[1].v == 0 and val or (data.showMax and data.battMin or data.batt)), MIDSIZE + RIGHT + tmp) - lcd.drawText(0, TOP + 51, labels[2], SMLSIZE) - local red = val >= config[2].v and math.max(math.floor((4.2 - val) / (4.2 - config[2].v) * 255), 0) or 255 - local green = val < config[2].v and math.max(math.floor((val - config[3].v) / (config[2].v - config[3].v) * 255), 0) or 255 - lcd.setColor(CUSTOM_COLOR, lcd.RGB(red, green, 60)) - lcd.drawGauge(0, TOP + 68, X1 - 3, 15, math.min(math.max(val - config[3].v + 0.1, 0) * (100 / (4.2 - config[3].v + 0.1)), 99), 100, CUSTOM_COLOR) + local val = math.floor((data.showMax and data.cellMin or data.cell) * 100 + 0.5) / 100 + text(X1 - 3, TOP + 42, fmt(config[1].v == 0 and "%.2fV" or "%.1fV", config[1].v == 0 and val or (data.showMax and data.battMin or data.batt)), MIDSIZE + RIGHT + tmp) + text(0, TOP + 51, labels[2], SMLSIZE) + if data.bl ~= val then + local red = val >= config[2].v and max(floor((4.2 - val) / (4.2 - config[2].v) * 255), 0) or 255 + local green = val < config[2].v and max(floor((val - config[3].v) / (config[2].v - config[3].v) * 255), 0) or 255 + data.bc = rgb(red, green, 60) + data.bl = val + end + color(CUSTOM_COLOR, data.bc) + lcd.drawGauge(0, TOP + 68, X1 - 3, 15, min(max(val - config[3].v + 0.1, 0) * (100 / (4.2 - config[3].v + 0.1)), 99), 100, CUSTOM_COLOR) tmp = (not data.telem or data.rssi < data.rssiLow) and FLASH or 0 val = data.showMax and data.rssiMin or data.rssiLast - lcd.drawText(X1 - 3, TOP + 84, val .. (data.crsf and "%" or "dB"), MIDSIZE + RIGHT + tmp) - lcd.drawText(0, TOP + 93, data.crsf and "LQ" or "RSSI", SMLSIZE) - local red = val >= data.rssiLow and math.max(math.floor((100 - val) / (100 - data.rssiLow) * 255), 0) or 255 - local green = val < data.rssiLow and math.max(math.floor((val - data.rssiCrit) / (data.rssiLow - data.rssiCrit) * 255), 0) or 255 - lcd.setColor(CUSTOM_COLOR, lcd.RGB(red, green, 60)) - lcd.drawGauge(0, TOP + 110, X1 - 3, 15, math.min(val, 99), 100, CUSTOM_COLOR) + text(X1 - 3, TOP + 84, val .. (data.crsf and "%" or "dB"), MIDSIZE + RIGHT + tmp) + text(0, TOP + 93, data.crsf and "LQ" or "RSSI", SMLSIZE) + if data.rl ~= val then + local red = val >= data.rssiLow and max(floor((100 - val) / (100 - data.rssiLow) * 255), 0) or 255 + local green = val < data.rssiLow and max(floor((val - data.rssiCrit) / (data.rssiLow - data.rssiCrit) * 255), 0) or 255 + data.rc = rgb(red, green, 60) + data.rl = val + end + color(CUSTOM_COLOR, data.rc) + lcd.drawGauge(0, TOP + 110, X1 - 3, 15, min(val, 99), 100, CUSTOM_COLOR) -- Box 2 (altitude, distance, current) tmp = data.showMax and data.altitudeMax or data.altitude - lcd.drawText(X1 + 9, TOP + 1, labels[4], SMLSIZE) - lcd.drawText(X2, TOP + 12, math.floor(tmp + 0.5) .. units[data.alt_unit], MIDSIZE + RIGHT + ((not data.telem or tmp + 0.5 >= config[6].v) and FLASH or 0)) - lcd.drawText(X1 + 9, TOP + 44, labels[5], SMLSIZE) - lcd.drawText(X2, TOP + 55, dist, MIDSIZE + RIGHT + data.telemFlags) + text(X1 + 9, TOP + 1, labels[4], SMLSIZE) + text(X2, TOP + 12, floor(tmp + 0.5) .. units[data.alt_unit], MIDSIZE + RIGHT + ((not data.telem or tmp + 0.5 >= config[6].v) and FLASH or 0)) + tmp2 = data.showMax and data.distanceMax or data.distanceLast + tmp = tmp2 < 1000 and floor(tmp2 + 0.5) .. units[data.dist_unit] or (fmt("%.1f", tmp2 / (data.dist_unit == 9 and 1000 or 5280)) .. (data.dist_unit == 9 and "km" or "mi")) + text(X1 + 9, TOP + 44, labels[5], SMLSIZE) + text(X2, TOP + 55, tmp, MIDSIZE + RIGHT + data.telemFlags) if data.showCurr then tmp = data.showMax and data.currentMax or data.current - lcd.drawText(X1 + 9, TOP + 87, labels[3], SMLSIZE) - lcd.drawText(X2, TOP + 98, (tmp >= 99.5 and math.floor(tmp + 0.5) or string.format("%.1fA", tmp)), MIDSIZE + RIGHT + data.telemFlags) + text(X1 + 9, TOP + 87, labels[3], SMLSIZE) + text(X2, TOP + 98, (tmp >= 99.5 and floor(tmp + 0.5) or fmt("%.1fA", tmp)), MIDSIZE + RIGHT + data.telemFlags) end -- Box 3 (flight modes, orientation) - lcd.drawText(X2 + 20, TOP, modes[data.modeId].t, modes[data.modeId].f == 3 and WARNING_COLOR or 0) + text(X2 + 20, TOP, modes[data.modeId].t, modes[data.modeId].f == 3 and WARNING_COLOR or 0) if data.altHold then - lcd.drawBitmap(icons.lock, X1 + 63, TOP + 4) + bmap(icons.lock, X1 + 63, TOP + 4) end if data.headFree then - lcd.drawText(X2 + 7, TOP + 19, "HF", FLASH) + text(X2 + 7, TOP + 19, "HF", FLASH) end if data.showHead then - if data.showDir or data.headingRef < 0 then - lcd.drawText((X2 + X3) / 2, TOP + 18, "N", SMLSIZE) - lcd.drawText(X3 - 4, 211, "E", SMLSIZE + RIGHT) - lcd.drawText(X2 + 10, 211, "W", SMLSIZE) - lcd.drawText(X2 + 78, BOTTOM - 15, math.floor(data.heading + 0.5) % 360 .. "\64", SMLSIZE + RIGHT + data.telemFlags) + if data.showDir or data.headingRef == -1 then + text((X2 + X3) / 2, TOP + 18, "N", SMLSIZE) + text(X3 - 4, 211, "E", SMLSIZE + RIGHT) + text(X2 + 10, 211, "W", SMLSIZE) + text(X2 + 78, BOTTOM - 15, floor(data.heading + 0.5) % 360 .. "\64", SMLSIZE + RIGHT + data.telemFlags) end local x1, y1, x2, y2, x3, y3 = calcDir(r1, r2, r3, (X2 + X3) / 2 + 4, 219, 25) if data.headingHold then - lcd.drawFilledRectangle((x2 + x3) / 2 - 2, (y2 + y3) / 2 - 2, 5, 5, SOLID) + fill((x2 + x3) / 2 - 2, (y2 + y3) / 2 - 2, 5, 5, SOLID) else - lcd.setColor(CUSTOM_COLOR, GREY) - lcd.drawLine(x2, y2, x3, y3, SOLID, CUSTOM_COLOR) + color(CUSTOM_COLOR, GREY) + line(x2, y2, x3, y3, SOLID, CUSTOM_COLOR) end - lcd.drawLine(x1, y1, x2, y2, SOLID, TEXT_COLOR) - lcd.drawLine(x1, y1, x3, y3, SOLID, TEXT_COLOR) + line(x1, y1, x2, y2, SOLID, TEXT_COLOR) + line(x1, y1, x3, y3, SOLID, TEXT_COLOR) end -- Box 4 (GPS info, speed) if data.crsf then if data.tpwr then - lcd.drawText(RIGHT_POS, TOP, data.tpwr .. "mW", RIGHT + MIDSIZE + data.telemFlags) + text(RIGHT_POS, TOP, data.tpwr .. "mW", RIGHT + MIDSIZE + data.telemFlags) end - lcd.drawText(RIGHT_POS + 1, TOP + 28, data.satellites % 100, MIDSIZE + RIGHT + data.telemFlags) + text(RIGHT_POS + 1, TOP + 28, data.satellites % 100, MIDSIZE + RIGHT + data.telemFlags) else tmp = ((data.armed or data.modeId == 6) and data.hdop < 11 - config[21].v * 2) or not data.telem - lcd.drawText(X3 + 48, TOP, (data.hdop == 0 and not data.gpsFix) and "-- --" or (9 - data.hdop) / 2 + 0.8, MIDSIZE + RIGHT + (tmp and FLASH or 0)) - lcd.drawText(X3 + 11, TOP + 24, "HDOP", SMLSIZE) - lcd.drawText(RIGHT_POS + 1, TOP, data.satellites % 100, MIDSIZE + RIGHT + data.telemFlags) + text(X3 + 48, TOP, (data.hdop == 0 and not data.gpsFix) and "-- --" or (9 - data.hdop) / 2 + 0.8, MIDSIZE + RIGHT + (tmp and FLASH or 0)) + text(X3 + 11, TOP + 24, "HDOP", SMLSIZE) + text(RIGHT_POS + 1, TOP, data.satellites % 100, MIDSIZE + RIGHT + data.telemFlags) end hdopGraph(X3 + 65, TOP + (data.crsf and 51 or 23)) tmp = RIGHT + ((not data.telem or not data.gpsFix) and FLASH or 0) if not data.crsf then - lcd.drawText(RIGHT_POS, TOP + 28, math.floor(data.gpsAlt + 0.5) .. (data.gpsAlt_unit == 10 and "'" or units[data.gpsAlt_unit]), MIDSIZE + tmp) + text(RIGHT_POS, TOP + 28, floor(data.gpsAlt + 0.5) .. (data.gpsAlt_unit == 10 and "'" or units[data.gpsAlt_unit]), MIDSIZE + tmp) end - lcd.drawText(RIGHT_POS, TOP + 54, config[16].v == 0 and string.format("%.6f", data.gpsLatLon.lat) or gpsDegMin(data.gpsLatLon.lat, true), tmp) - lcd.drawText(RIGHT_POS, TOP + 74, config[16].v == 0 and string.format("%.6f", data.gpsLatLon.lon) or gpsDegMin(data.gpsLatLon.lon, false), tmp) + text(RIGHT_POS, TOP + 54, config[16].v == 0 and fmt("%.6f", data.gpsLatLon.lat) or gpsDegMin(data.gpsLatLon.lat, true), tmp) + text(RIGHT_POS, TOP + 74, config[16].v == 0 and fmt("%.6f", data.gpsLatLon.lon) or gpsDegMin(data.gpsLatLon.lon, false), tmp) tmp = data.showMax and data.speedMax or data.speed - lcd.drawText(RIGHT_POS + 1, TOP + 98, tmp >= 99.5 and math.floor(tmp + 0.5) .. units[data.speed_unit] or string.format("%.1f", tmp) .. units[data.speed_unit], MIDSIZE + RIGHT + data.telemFlags) + text(RIGHT_POS + 1, TOP + 98, tmp >= 99.5 and floor(tmp + 0.5) .. units[data.speed_unit] or fmt("%.1f", tmp) .. units[data.speed_unit], MIDSIZE + RIGHT + data.telemFlags) -- Dividers - lcd.setColor(CUSTOM_COLOR, GREY) - lcd.drawLine(X1 + 3, TOP, X1 + 3, BOTTOM, DOTTED, CUSTOM_COLOR) - lcd.drawLine(X2 + 3, TOP, X2 + 3, BOTTOM, DOTTED, CUSTOM_COLOR) - lcd.drawLine(X3 + 3, TOP, X3 + 3, BOTTOM, DOTTED, CUSTOM_COLOR) - lcd.drawLine(X3 + 3, TOP + 95, RIGHT_POS, TOP + 95, DOTTED, CUSTOM_COLOR) + color(CUSTOM_COLOR, GREY) + line(X1 + 3, TOP, X1 + 3, BOTTOM, DOTTED, CUSTOM_COLOR) + line(X2 + 3, TOP, X2 + 3, BOTTOM, DOTTED, CUSTOM_COLOR) + line(X3 + 3, TOP, X3 + 3, BOTTOM, DOTTED, CUSTOM_COLOR) + line(X3 + 3, TOP + 95, RIGHT_POS, TOP + 95, DOTTED, CUSTOM_COLOR) if data.crsf then - lcd.drawLine(X3 + 3, TOP + 28, RIGHT_POS, TOP + 28, DOTTED, CUSTOM_COLOR) + line(X3 + 3, TOP + 28, RIGHT_POS, TOP + 28, DOTTED, CUSTOM_COLOR) end - lcd.setColor(CUSTOM_COLOR, LIGHTGREY) - lcd.drawLine(0, TOP - 1, LCD_W - 1, TOP - 1, SOLID, CUSTOM_COLOR) + color(CUSTOM_COLOR, LIGHTGREY) + line(0, TOP - 1, LCD_W - 1, TOP - 1, SOLID, CUSTOM_COLOR) if data.showMax then - lcd.setColor(CUSTOM_COLOR, YELLOW) - lcd.drawFilledRectangle(190, TOP - 20, 80, 20, CUSTOM_COLOR) - lcd.setColor(CUSTOM_COLOR, BLACK) - lcd.drawText(195, TOP - 20, "Min/Max", CUSTOM_COLOR) + color(CUSTOM_COLOR, YELLOW) + fill(190, TOP - 20, 80, 20, CUSTOM_COLOR) + color(CUSTOM_COLOR, BLACK) + text(195, TOP - 20, "Min/Max", CUSTOM_COLOR) end end diff --git a/src/iNav/lang.lua b/src/iNav/lang.lua index d459543f..08f7598c 100644 --- a/src/iNav/lang.lua +++ b/src/iNav/lang.lua @@ -1,4 +1,4 @@ -local modes, labels, data, FILE_PATH = ... +local modes, labels, data, FILE_PATH, env = ... local lang if data.lang ~= "en" then @@ -6,7 +6,7 @@ if data.lang ~= "en" then local fh = io.open(tmp) if fh ~= nil then io.close(fh) - lang = loadfile(tmp)(modes, labels) + lang = loadScript(tmp, env)(modes, labels) collectgarbage() end end diff --git a/src/iNav/lang_de.lua b/src/iNav/lang_de.lua index 04a47cf8..fc4de5fb 100644 --- a/src/iNav/lang_de.lua +++ b/src/iNav/lang_de.lua @@ -16,71 +16,71 @@ modes[12].t = " ! GAS ! " -- ! THROT ! --modes[13].t = " CRUISE" -- CRUISE -- Max 10 characters -labels[1] = "Kapazitaet"-- Fuel -labels[2] = "Batterie" -- Battery +labels[1] = "Kapazitaet" -- Fuel +labels[2] = "Batterie" -- Battery labels[3] = "Strom" -- Current labels[4] = "Hoehe" -- Altitude -labels[5] = "Entfernung"-- Distance +labels[5] = "Entfernung" -- Distance local function lang(config2) -- Max 16 characters - config2[1].t = "Batterie" -- Battery View + config2[1].t = "Batterie" -- Battery View config2[2].t = "Zelle Niedrig" -- Cell Low - config2[3].t = "Zelle Kritisch" -- Cell Critical + config2[3].t = "Zelle Kritisch" -- Cell Critical config2[4].t = "Sprach Alarme" -- Voice Alerts - config2[5].t = "Meldungen" -- Feedback - config2[6].t = "Max Hoehe" -- Max Altitude + config2[5].t = "Meldungen" -- Feedback + config2[6].t = "Max Hoehe" -- Max Altitude --config2[7].t = "Variometer" -- Variometer config2[8].t = "RTH Meldung" -- RTH Feedback - config2[9].t = "HeadFree Meld." -- HeadFree Feedback + config2[9].t = "HeadFree Meld." -- HeadFree Feedback config2[10].t = "RSSI Meldung" -- RSSI Feedback config2[11].t = "Bat. Warnung" -- Battery Alert - config2[12].t = "Hoehen Warnung" -- Altitude Alert - --config2[13].t = "Timer" -- Timer - config2[14].t = "Rx Spng." -- Rx Voltage - --config2[15].t = "GPS" -- GPS - config2[16].t = "GPS Koordi." -- GPS Coordinates + config2[12].t = "Hoehen Warnung" -- Altitude Alert + --config2[13].t = "Timer" -- Timer + config2[14].t = "Rx Spng." -- Rx Voltage + config2[15].t = "Flugpfad-Vektor" -- Flight Path Vector + --config2[16].t = "GPS" -- GPS config2[17].t = "Kapaz. Kritisch" -- Fuel Critical - config2[18].t = "Kapaz. Niedrig" -- Fuel Low - config2[19].t = "Tx Spng." -- Tx Voltage - config2[20].t = "Geschw. Sensor" -- Speed Sensor + config2[18].t = "Kapaz. Niedrig" -- Fuel Low + config2[19].t = "Tx Spng." -- Tx Voltage + config2[20].t = "Geschw. Sensor" -- Speed Sensor config2[21].t = "GPS Warnung" -- GPS Warning - config2[22].t = "GPS HDOP" -- GPS HDOP View - config2[23].t = "Kapazitaet" -- Fuel Unit - config2[24].t = "Vario Schritte" -- Vario Steps + config2[22].t = "GPS HDOP" -- GPS HDOP View + config2[23].t = "Kapazitaet" -- Fuel Unit + config2[24].t = "Vario Schritte" -- Vario Steps config2[25].t = "Ansichtsmodus" -- View Mode - config2[26].t = "AlH Cntr Meld." -- AltHold Center FB + config2[26].t = "AlH Cntr Meld." -- AltHold Center FB config2[27].t = "Bat. Kapazitaet" -- Battery Capacity config2[28].t = "Hoehenkurve" -- Altitude Graph config2[29].t = "Zellenberechnung" -- Cell Calculation - --config2[30].t = "Aircraft Symbol" -- Aircraft Symbol - --config2[31].t = "Center Map" -- Center Map Home - --config2[32].t = "Orientation" -- Orientation - --config2[33].t = "Roll Indicator" -- Roll Indicator + config2[30].t = "Flugzeug-Symbol" -- Aircraft Symbol + config2[31].t = "Karte Zentrieren" -- Center Map Home + config2[32].t = "Orientierung" -- Orientation + config2[33].t = "Rollenwaage" -- Roll Scale -- Max 8 characters - config2[1].l = {[0] = "Zelle", "Total"} -- "Cell", "Total" - config2[4].l = {[0] = "Aus", "Kritisch", "Alle"} -- "Off", "Critical", "All" - config2[5].l = {[0] = "Aus", "Haptisch", "Pieper", "Alle"} -- "Off", "Haptic", "Beeper", "All" - config2[7].l = {[0] = "Aus", "Graph", "Stimme", "Beides"} -- "Off", "Graph", "Voice", "Both" - config2[8].l = {[0] = "Aus", "An"} -- "Off", "On" - config2[9].l = {[0] = "Aus", "An"} -- "Off", "On" - config2[10].l = {[0] = "Aus", "An"} -- "Off", "On" - config2[11].l = {[0] = "Aus", "Kritisch", "Alle"} -- "Off", "Critical", "All" - config2[12].l = {[0] = "Aus", "An"} -- "Off", "On" - config2[13].l = {[0] = "Aus", "Auto", "Timer1", "Timer2"} -- "Off", "Auto", "Timer1", "Timer2" - config2[14].l = {[0] = "Aus", "An"} -- "Off", "On" - config2[16].l = {[0] = "Dezimal", "Grad"} -- "Decimal", "Deg/Min" - config2[19].l = {[0] = "Nummer", "Graph", "Beide"} -- "Number", "Graph", "Both" - --config2[20].l = {[0] = "GPS", "Pitot"} -- "GPS", "Pitot" - config2[22].l = {[0] = "Graph", "Dezimal"} -- "Graph", "Decimal" - --config2[23].l = {[0] = "Percent", "mAh", "mWh"} -- "Percent", "mAh", "mWh" + config2[1].l = {[0] = "Zelle", "Total"} -- "Cell", "Total" + config2[4].l = {[0] = "Aus", "Kritisch", "Alle"} -- "Off", "Critical", "All" + config2[5].l = {[0] = "Aus", "Haptisch", "Pieper", "Alle"} -- "Off", "Haptic", "Beeper", "All" + config2[7].l = {[0] = "Aus", "Graph", "Stimme", "Beides"} -- "Off", "Graph", "Voice", "Both" + config2[8].l = {[0] = "Aus", "An"} -- "Off", "On" + config2[9].l = {[0] = "Aus", "An"} -- "Off", "On" + config2[10].l = {[0] = "Aus", "An"} -- "Off", "On" + config2[11].l = {[0] = "Aus", "Kritisch", "Alle"} -- "Off", "Critical", "All" + config2[12].l = {[0] = "Aus", "An"} -- "Off", "On" + config2[13].l = {[0] = "Aus", "Auto", "Timer1", "Timer2"} -- "Off", "Auto", "Timer1", "Timer2" + config2[14].l = {[0] = "Aus", "An"} -- "Off", "On" + config2[16].l = {[0] = "Dezimal", "Grad"} -- "Decimal", "Deg/Min" + config2[19].l = {[0] = "Nummer", "Graph", "Beide"} -- "Number", "Graph", "Both" + --config2[20].l = {[0] = "GPS", "Pitot"} -- "GPS", "Pitot" + config2[22].l = {[0] = "Graph", "Dezimal"} -- "Graph", "Decimal" + --config2[23].l = {[0] = "Percent", "mAh", "mWh"} -- "Percent", "mAh", "mWh" config2[25].l = {[0] = "Original", "Pilot", "Radar", "Hoehe"} -- "Classic", "Pilot", "Radar", "Altitude" - config2[26].l = {[0] = "Aus", "An"} -- "Off", "On" - config2[28].l[0] = "Aus" -- "Off" - config2[31].l = {[0] = "Aus", "An"} -- "Off", "On" - --config2[32].l = {[0] = "Launch", "Compass"} -- "Launch", "Compass" - config2[33].l = {[0] = "Aus", "An"} -- "Off", "On" + config2[26].l = {[0] = "Aus", "An"} -- "Off", "On" + config2[28].l[0] = "Aus" -- "Off" + config2[31].l = {[0] = "Aus", "An"} -- "Off", "On" + config2[32].l = {[0] = "Starten", "Kompass"} -- "Launch", "Compass" + config2[33].l = {[0] = "Aus", "An"} -- "Off", "On" end return lang \ No newline at end of file diff --git a/src/iNav/lang_es.lua b/src/iNav/lang_es.lua index 9705d66d..e453c6b5 100644 --- a/src/iNav/lang_es.lua +++ b/src/iNav/lang_es.lua @@ -16,71 +16,71 @@ modes[12].t = " ! GAS ! " -- ! THROT ! --modes[13].t = " CRUISE" -- CRUISE -- Max 10 characters -labels[1] = "Bateria" -- Fuel -labels[2] = "Tension" -- Battery -labels[3] = "Consumo" -- Current -labels[4] = "Altitud" -- Altitude -labels[5] = "Distancia" -- Distance +labels[1] = "Bateria" -- Fuel +labels[2] = "Tension" -- Battery +labels[3] = "Consumo" -- Current +labels[4] = "Altitud" -- Altitude +labels[5] = "Distancia" -- Distance local function lang(config2) -- Max 16 characters - config2[1].t = "Bateria" -- Battery View - config2[2].t = "Celda baja" -- Cell Low + config2[1].t = "Bateria" -- Battery View + config2[2].t = "Celda baja" -- Cell Low config2[3].t = "Celda Critica" -- Cell Critical config2[4].t = "Alerta Voces" -- Voice Alerts - --config2[5].t = "Feedback" -- Feedback - config2[6].t = "Max Altura" -- Max Altitude - config2[7].t = "Variometro" -- Variometer - --config2[8].t = "RTH Feedback" -- RTH Feedback - --config2[9].t = "HeadFree Feedback"-- HeadFree Feedback + --config2[5].t = "Feedback" -- Feedback + config2[6].t = "Max Altura" -- Max Altitude + config2[7].t = "Variometro" -- Variometer + --config2[8].t = "RTH Feedback" -- RTH Feedback + --config2[9].t = "HeadFree Feedback" -- HeadFree Feedback --config2[10].t = "RSSI Feedback" -- RSSI Feedback - config2[11].t = "Bateria Alerta" -- Battery Alert - config2[12].t = "Altitud Alerta" -- Altitude Alert - --config2[13].t = "Timer" -- Timer - config2[14].t = "Rx Voltaje" -- Rx Voltage - --config2[15].t = "GPS" -- GPS - config2[16].t = "Coordenadas GPS" -- GPS Coordinates + config2[11].t = "Bateria Alerta" -- Battery Alert + config2[12].t = "Altitud Alerta" -- Altitude Alert + --config2[13].t = "Timer" -- Timer + config2[14].t = "Rx Voltaje" -- Rx Voltage + config2[15].t = "Vuelo Ruta Vector" -- Flight Path Vector + --config2[16].t = "GPS" -- GPS config2[17].t = "Bateria Critica" -- Fuel Critical config2[18].t = "Bateria Baja" -- Fuel Low - config2[19].t = "Tx Voltaje" -- Tx Voltage + config2[19].t = "Tx Voltaje" -- Tx Voltage config2[20].t = "Sensor Velocidad" -- Speed Sensor - config2[21].t = "GPS Aviso" -- GPS Warning - config2[22].t = "GPS HDOP" -- GPS HDOP View - config2[23].t = "Capacidad" -- Fuel Unit + config2[21].t = "GPS Aviso" -- GPS Warning + config2[22].t = "GPS HDOP" -- GPS HDOP View + config2[23].t = "Capacidad" -- Fuel Unit config2[24].t = "Vario Pasos" -- Vario Steps - config2[25].t = "Vista" -- View Mode + config2[25].t = "Vista" -- View Mode config2[26].t = "AlH Centrado FB." -- AltHold Center FB config2[27].t = "Capacidad Bateria" -- Battery Capacity - config2[28].t = "Grafico Altura" -- Altitude Graph + config2[28].t = "Grafico Altura" -- Altitude Graph config2[29].t = "Calculo Celda" -- Cell Calculation - --config2[30].t = "Aircraft Symbol" -- Aircraft Symbol - --config2[31].t = "Center Map" -- Center Map Home - --config2[32].t = "Orientation" -- Orientation - --config2[33].t = "Roll Indicator" -- Roll Indicator + config2[30].t = "Simbolo Aeronave" -- Aircraft Symbol + config2[31].t = "Mapa del Centro" -- Center Map Home + config2[32].t = "Orientacion" -- Orientation + --config2[33].t = "Roll Scale" -- Roll Scale -- Max 8 characters - --config2[1].l = {[0] = "Cell", "Total"} -- "Cell", "Total" - config2[4].l = {[0] = "Off", "Critico", "Todo"} -- "Off", "Critical", "All" - config2[5].l = {[0] = "Off", "Haptic", "Beeper", "Todo"} -- "Off", "Haptic", "Beeper", "All" - config2[7].l = {[0] = "Off", "Grafico", "Voz", "Ambos"} -- "Off", "Graph", "Voice", "Both" - --config2[8].l = {[0] = "Off", "On"} -- "Off", "On" - --config2[9].l = {[0] = "Off", "On"} -- "Off", "On" - --config2[10].l = {[0] = "Off", "On"} -- "Off", "On" - config2[11].l = {[0] = "Off", "Critico", "Todo"} -- "Off", "Critical", "All" - --config2[12].l = {[0] = "Off", "On"} -- "Off", "On" + --config2[1].l = {[0] = "Cell", "Total"} -- "Cell", "Total" + config2[4].l = {[0] = "Off", "Critico", "Todo"} -- "Off", "Critical", "All" + config2[5].l = {[0] = "Off", "Haptic", "Beeper", "Todo"} -- "Off", "Haptic", "Beeper", "All" + config2[7].l = {[0] = "Off", "Grafico", "Voz", "Ambos"} -- "Off", "Graph", "Voice", "Both" + --config2[8].l = {[0] = "Off", "On"} -- "Off", "On" + --config2[9].l = {[0] = "Off", "On"} -- "Off", "On" + --config2[10].l = {[0] = "Off", "On"} -- "Off", "On" + config2[11].l = {[0] = "Off", "Critico", "Todo"} -- "Off", "Critical", "All" + --config2[12].l = {[0] = "Off", "On"} -- "Off", "On" --config2[13].l = {[0] = "Off", "Auto", "Timer1", "Timer2"} -- "Off", "Auto", "Timer1", "Timer2" - --config2[14].l = {[0] = "Off", "On"} -- "Off", "On" - config2[16].l = {[0] = "Decimal", "Grados"} -- "Decimal", "Deg/Min" - config2[19].l = {[0] = "Numero", "Grafico", "Ambos"} -- "Number", "Graph", "Both" - --config2[20].l = {[0] = "GPS", "Pitot"} -- "GPS", "Pitot" - config2[22].l = {[0] = "Grafico", "Decimal"} -- "Graph", "Decimal" - --config2[23].l = {[0] = "Percent", "mAh", "mWh"} -- "Percent", "mAh", "mWh" + --config2[14].l = {[0] = "Off", "On"} -- "Off", "On" + config2[16].l = {[0] = "Decimal", "Grados"} -- "Decimal", "Deg/Min" + config2[19].l = {[0] = "Numero", "Grafico", "Ambos"} -- "Number", "Graph", "Both" + --config2[20].l = {[0] = "GPS", "Pitot"} -- "GPS", "Pitot" + config2[22].l = {[0] = "Grafico", "Decimal"} -- "Graph", "Decimal" + --config2[23].l = {[0] = "Percent", "mAh", "mWh"} -- "Percent", "mAh", "mWh" config2[25].l = {[0] = "Clasica", "Pilot", "Radar", "Altitud"} -- "Classic", "Pilot", "Radar", "Altitude" - --config2[26].l = {[0] = "Off", "On"} -- "Off", "On" - --config2[28].l[0] = "Off" -- "Off" - --config2[31].l = {[0] = "Off", "On"} -- "Off", "On" - --config2[32].l = {[0] = "Launch", "Compass"} -- "Launch", "Compass" - --config2[33].l = {[0] = "Off", "On"} -- "Off", "On" + --config2[26].l = {[0] = "Off", "On"} -- "Off", "On" + --config2[28].l[0] = "Off" -- "Off" + --config2[31].l = {[0] = "Off", "On"} -- "Off", "On" + config2[32].l = {[0] = "Brazo", "Brujula"} -- "Launch", "Compass" + --config2[33].l = {[0] = "Off", "On"} -- "Off", "On" end return lang \ No newline at end of file diff --git a/src/iNav/lang_fr.lua b/src/iNav/lang_fr.lua index d200bd70..f1c28cd1 100644 --- a/src/iNav/lang_fr.lua +++ b/src/iNav/lang_fr.lua @@ -28,59 +28,59 @@ local function lang(config2) config2[2].t = "Cellule Basse" -- Cell Low config2[3].t = "Cellule Critique" -- Cell Critical config2[4].t = "Alerte Vocale" -- Voice Alerts - --config2[5].t = "Feedback" -- Feedback + --config2[5].t = "Feedback" -- Feedback config2[6].t = "Altitude Maximum" -- Max Altitude - config2[7].t = "Variometre" -- Variometer + config2[7].t = "Variometre" -- Variometer config2[8].t = "Feedback RTH" -- RTH Feedback config2[9].t = "Feedback HeadFree" -- HeadFree Feedback config2[10].t = "Feedback RSSI" -- RSSI Feedback config2[11].t = "Alerte Batterie" -- Battery Alert config2[12].t = "Alerte Altitude" -- Altitude Alert config2[13].t = "Chronometre" -- Timer - config2[14].t = "Voltage Rx" -- Rx Voltage - --config2[15].t = "GPS" -- GPS - config2[16].t = "Coordonnees GPS" -- GPS Coordinates + config2[14].t = "Voltage Rx" -- Rx Voltage + config2[15].t = "Vecteur de Vol" -- Flight Path Vector + --config2[16].t = "GPS" -- GPS config2[17].t = "Fuel Critique" -- Fuel Critical - config2[18].t = "Fuel Bas" -- Fuel Low - config2[19].t = "Voltage Tx" -- Tx Voltage + config2[18].t = "Fuel Bas" -- Fuel Low + config2[19].t = "Voltage Tx" -- Tx Voltage config2[20].t = "Capteur Vitesse" -- Speed Sensor - config2[21].t = "Alerte GPS" -- GPS Warning + config2[21].t = "Alerte GPS" -- GPS Warning config2[22].t = "Vue GPS HDOP" -- GPS HDOP View - config2[23].t = "Unite Fuel" -- Fuel Unit + config2[23].t = "Unite Fuel" -- Fuel Unit config2[24].t = "Avance Vario" -- Vario Steps config2[25].t = "Mode de Vue" -- View Mode config2[26].t = "Feedback AltHold" -- AltHold Center FB config2[27].t = "Capacite Batterie" -- Battery Capacity config2[28].t = "Graphique d'Alt" -- Altitude Graph config2[29].t = "Calcul de Cellule" -- Cell Calculation - --config2[30].t = "Aircraft Symbol" -- Aircraft Symbol - --config2[31].t = "Center Map" -- Center Map Home + config2[30].t = "Symbole D'avion" -- Aircraft Symbol + config2[31].t = "Carte du Centre" -- Center Map Home --config2[32].t = "Orientation" -- Orientation - --config2[33].t = "Roll Indicator" -- Roll Indicator + config2[33].t = "Echelle Rouleau" -- Roll Scale -- Max 8 characters - config2[1].l = {[0] = "Cellule", "Total"} -- "Cell", "Total" - config2[4].l = {[0] = "Eteint", "Critique", "Tous"} -- "Off", "Critical", "All" + config2[1].l = {[0] = "Cellule", "Total"} -- "Cell", "Total" + config2[4].l = {[0] = "Eteint", "Critique", "Tous"} -- "Off", "Critical", "All" config2[5].l = {[0] = "Eteint", "Haptique", "Beeper", "Tous"} -- "Off", "Haptic", "Beeper", "All" config2[7].l = {[0] = "Eteint", "Graph", "Voix", "Les deux"} -- "Off", "Graph", "Voice", "Both" - config2[8].l = {[0] = "Eteint", "Actif"} -- "Off", "On" - config2[9].l = {[0] = "Eteint", "Actif"} -- "Off", "On" - config2[10].l = {[0] = "Eteint", "Actif"} -- "Off", "On" - config2[11].l = {[0] = "Eteint", "Critique", "Tous"} -- "Off", "Critical", "All" - config2[12].l = {[0] = "Eteint", "Actif"} -- "Off", "On" + config2[8].l = {[0] = "Eteint", "Actif"} -- "Off", "On" + config2[9].l = {[0] = "Eteint", "Actif"} -- "Off", "On" + config2[10].l = {[0] = "Eteint", "Actif"} -- "Off", "On" + config2[11].l = {[0] = "Eteint", "Critique", "Tous"} -- "Off", "Critical", "All" + config2[12].l = {[0] = "Eteint", "Actif"} -- "Off", "On" config2[13].l = {[0] = "Eteint", "Auto", "Chrono1", "Chrono2"} -- "Off", "Auto", "Timer1", "Timer2" - config2[14].l = {[0] = "Eteint", "Actif"} -- "Off", "On" - --config2[16].l = {[0] = "Decimal", "Deg/Min"} -- "Decimal", "Deg/Min" - config2[19].l = {[0] = "Numerique", "Graph", "Les deux"} -- "Number", "Graph", "Both" - --config2[20].l = {[0] = "GPS", "Pitot"} -- "GPS", "Pitot" - config2[22].l = {[0] = "Graph", "Decimal"} -- "Graph", "Decimal" - config2[23].l = {[0] = "Pourcent", "mAh", "mWh"} -- "Percent", "mAh", "mWh" + config2[14].l = {[0] = "Eteint", "Actif"} -- "Off", "On" + --config2[16].l = {[0] = "Decimal", "Deg/Min"} -- "Decimal", "Deg/Min" + config2[19].l = {[0] = "Numerique", "Graph", "Les deux"} -- "Number", "Graph", "Both" + --config2[20].l = {[0] = "GPS", "Pitot"} -- "GPS", "Pitot" + config2[22].l = {[0] = "Graph", "Decimal"} -- "Graph", "Decimal" + config2[23].l = {[0] = "Pourcent", "mAh", "mWh"} -- "Percent", "mAh", "mWh" config2[25].l = {[0] = "Classique", "Pilote", "Radar", "Altitude"}-- "Classic", "Pilot", "Radar", "Altitude" - config2[26].l = {[0] = "Eteint", "Actif"} -- "Off", "On" - config2[28].l[0] = "Eteint" -- "Off" - config2[31].l = {[0] = "Eteint", "Actif"} -- "Off", "On" - --config2[32].l = {[0] = "Launch", "Compass"} -- "Launch", "Compass" - config2[33].l = {[0] = "Eteint", "Actif"} -- "Off", "On" + config2[26].l = {[0] = "Eteint", "Actif"} -- "Off", "On" + config2[28].l[0] = "Eteint" -- "Off" + config2[31].l = {[0] = "Eteint", "Actif"} -- "Off", "On" + config2[32].l = {[0] = "Bras", "Boussole"} -- "Launch", "Compass" + config2[33].l = {[0] = "Eteint", "Actif"} -- "Off", "On" end return lang \ No newline at end of file diff --git a/src/iNav/menu.lua b/src/iNav/menu.lua index 252d2747..6fd18fde 100644 --- a/src/iNav/menu.lua +++ b/src/iNav/menu.lua @@ -3,7 +3,7 @@ local function view(data, config, units, lang, event, gpsDegMin, getTelemetryId, local CONFIG_X = HORUS and 90 or (SMLCD and 0 or 46) local TOP = HORUS and 37 or 11 local LINE = HORUS and 22 or 9 - local RIGHT = HORUS and 200 or 83 + local RSIDE = HORUS and 200 or 83 local GPS = HORUS and 45 or 21 local ROWS = HORUS and 9 or 5 local FONT = HORUS and 0 or SMLSIZE @@ -24,8 +24,8 @@ local function view(data, config, units, lang, event, gpsDegMin, getTelemetryId, { t = "Altitude Alert", l = {[0] = "Off", "On"} }, -- 12 { t = "Timer", l = {[0] = "Off", "Auto", "Timer1", "Timer2"} }, -- 13 { t = "Rx Voltage", l = {[0] = "Off", "On"} }, -- 14 - { t = "GPS", i = 0, l = {[0] = data.lastLock} }, -- 15 - { t = "GPS Coordinates", l = {[0] = "Decimal", "Deg/Min"} }, -- 16 + { t = "Flight Vector", l = {[0] = "Off", "On"} }, -- 15 + { t = "GPS", l = {[0] = string.format("%10.6f %11.6f", data.lastLock.lat, data.lastLock.lon), gpsDegMin(data.lastLock.lat, true) .. " " .. gpsDegMin(data.lastLock.lon, false)} }, -- 16 { t = "Fuel Critical", m = 1, a = "%" }, -- 17 { t = "Fuel Low", m = 2, a = "%" }, -- 18 { t = "Tx Voltage", l = {[0] = "Number", "Graph", "Both"} }, -- 19 @@ -42,7 +42,7 @@ local function view(data, config, units, lang, event, gpsDegMin, getTelemetryId, { t = "Aircraft Symbol", a = "" }, -- 30 { t = "Center Map Home", l = {[0] = "Off", "On"} }, -- 31 { t = "Orientation", l = {[0] = "Launch", "Compass"} }, -- 32 - { t = "Roll Indicator", l = {[0] = "Off", "On"} }, -- 33 + { t = "Roll Scale", l = {[0] = "Off", "On"} }, -- 33 } if lang ~= nil then @@ -70,6 +70,14 @@ local function view(data, config, units, lang, event, gpsDegMin, getTelemetryId, lcd.setColor(CUSTOM_COLOR, GREY) lcd.drawFilledRectangle(CONFIG_X - 10, TOP - 7, LCD_W - CONFIG_X * 2 + 20, LINE * (ROWS + 1) + 12, CUSTOM_COLOR) lcd.setColor(CUSTOM_COLOR, 12678) -- Dark grey + lcd.drawFilledRectangle(0, TOP - 7, 75, (LINE * (data.crsf and 1 or 2)) + 14, CUSTOM_COLOR) + lcd.drawRectangle(0, TOP - 7, 75, (LINE * (data.crsf and 1 or 2)) + 14, TEXT_COLOR) + lcd.drawText(4, TOP, "Sats:", FONT) + lcd.drawText(72, TOP, data.satellites % 100, FONT + RIGHT) + if not data.crsf then + lcd.drawText(4, TOP + LINE, "DOP:", FONT) + lcd.drawText(72, TOP + LINE, (data.hdop == 0 and not data.gpsFix) and "---" or (9 - data.hdop) / 2 + 0.8, FONT + RIGHT) + end end if not SMLCD then lcd.drawRectangle(CONFIG_X - (HORUS and 10 or 3), TOP - (HORUS and 7 or 2), LCD_W - CONFIG_X * 2 + (HORUS and 20 or 6), LINE * (ROWS + 1) + (HORUS and 12 or 1), SOLID) @@ -85,6 +93,7 @@ local function view(data, config, units, lang, event, gpsDegMin, getTelemetryId, -- Disabled options config2[7].p = data.crsf and 1 or (data.vspeed_id == -1 and 1 or nil) + config2[15].p = not data.crsf and 1 or (not HORUS and 1 or nil) config2[20].p = not data.pitot and 1 or nil config2[22].p = data.crsf and 1 or (HORUS and 1 or nil) config2[23].p = not data.showFuel and 1 or nil @@ -136,20 +145,16 @@ local function view(data, config, units, lang, event, gpsDegMin, getTelemetryId, lcd.drawText(CONFIG_X, y, config2[z].t, FONT + (config2[z].p == 1 and tmp or 0)) if config2[z].p == nil then if config2[z].l == nil then - lcd.drawText(CONFIG_X + RIGHT, y, (config[z].d ~= nil and string.format("%.1f", config[z].v) or config[z].v) .. config2[z].a, FONT + tmp) + lcd.drawText(CONFIG_X + RSIDE, y, (config[z].d ~= nil and string.format("%.1f", config[z].v) or config[z].v) .. config2[z].a, FONT + tmp) else if not config2[z].l then - lcd.drawText(CONFIG_X + RIGHT, y, config[z].v, FONT + tmp) + lcd.drawText(CONFIG_X + RSIDE, y, config[z].v, FONT + tmp) else - if z == 15 then - lcd.drawText(CONFIG_X + GPS, y, config[16].v == 0 and string.format("%10.6f %11.6f", config2[z].l[config[z].v].lat, config2[z].l[config[z].v].lon) or " " .. gpsDegMin(config2[z].l[config[z].v].lat, true) .. " " .. gpsDegMin(config2[z].l[config[z].v].lon, false), FONT + tmp) - else - lcd.drawText(CONFIG_X + RIGHT, y, config2[z].l[config[z].v] .. ((config2[z].a == nil or config[z].v == 0) and "" or config2[z].a), FONT + tmp) - end + lcd.drawText(z == 16 and LCD_W - CONFIG_X or CONFIG_X + RSIDE, y, config2[z].l[config[z].v] .. ((config2[z].a == nil or config[z].v == 0) and "" or config2[z].a), FONT + tmp + (z == 16 and RIGHT or 0)) end end else - lcd.drawText(CONFIG_X + RIGHT, y, "--", FONT + tmp) + lcd.drawText(CONFIG_X + RSIDE, y, "--", FONT + tmp) end end diff --git a/src/iNav/other.lua b/src/iNav/other.lua index b99642b8..318ea938 100644 --- a/src/iNav/other.lua +++ b/src/iNav/other.lua @@ -1,4 +1,4 @@ -local config, data, units, getTelemetryId, getTelemetryUnit, FILE_PATH = ... +local config, data, units, getTelemetryId, getTelemetryUnit, FILE_PATH, env = ... local crsf = nil -- Detect Crossfire @@ -8,7 +8,7 @@ data.fm_id = getTelemetryId("FM") > -1 and getTelemetryId("FM") or getTelemetryI --if data.simu then data.fm_id = 1 end if data.fm_id > -1 then - crsf = loadfile(FILE_PATH .. "crsf.luac")(config, data, getTelemetryId) + crsf = loadScript(FILE_PATH .. "crsf.luac", env)(config, data, getTelemetryId) collectgarbage() end @@ -61,4 +61,29 @@ end -- Use timer3 for flight reset detection model.setTimer(2, { mode = 0, start = 0, value = 3600, countdownBeep = 0, minuteBeep = false, persistent = 0} ) -return crsf \ No newline at end of file +-- Calculate distance to home if sensor is missing or in simlulator +local distCalc = nil +if data.dist_id == -1 or data.simu then + function distCalc(data) + local rad = math.rad + local o1 = rad(data.gpsHome.lat) + local o2 = rad(data.gpsLatLon.lat) + --[[ Spherical-Earth math: More accurate if the Earth was a sphere, but it's not so who cares? + data.distance = math.acos(math.sin(o1) * math.sin(o2) + math.cos(o1) * math.cos(o2) * math.cos(rad(data.gpsLatLon.lon) - rad(data.gpsHome.lon))) * 6371009 + ]] + -- Flat-Earth math + local x = (rad(data.gpsLatLon.lon) - rad(data.gpsHome.lon)) * math.cos((o1 + o2) / 2) + local y = o2 - o1 + data.distance = math.sqrt(x * x + y * y) * 6371009 + data.distanceMax = math.max(data.distMaxCalc, data.distance) + data.distMaxCalc = data.distanceMax + -- If distance is in feet, convert + if data.dist_unit == 10 then + data.distance = math.floor(data.distance * 3.28084 + 0.5) + data.distanceMax = data.distanceMax * 3.28084 + end + return 0 + end +end + +return crsf, distCalc \ No newline at end of file diff --git a/src/iNav/pilot.lua b/src/iNav/pilot.lua index 2002c2f5..013cdc4c 100644 --- a/src/iNav/pilot.lua +++ b/src/iNav/pilot.lua @@ -1,4 +1,4 @@ -local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcTrig, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) +local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcBearing, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) local LEFT_POS = SMLCD and 0 or 36 local RIGHT_POS = SMLCD and LCD_W - 31 or LCD_W - 53 @@ -71,7 +71,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic end -- Orientation - if data.telem and data.headingRef >= 0 and data.startup == 0 then + if data.telem and data.headingRef ~= -1 and data.startup == 0 then local x = LEFT_POS + 13.5 local r1 = math.rad(data.heading - data.headingRef) local r2 = math.rad(data.heading - data.headingRef + 145) @@ -116,7 +116,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic if data.showHead and data.armed and data.telem and data.gpsHome ~= false and data.startup == 0 and ((SMLCD and not data.showDir) or not SMLCD) then local home = X_CNTR - 3 if data.distanceLast >= data.distRef then - local bearing = calcTrig(data.gpsHome, data.gpsLatLon, true) + 540 % 360 + local bearing = calcBearing(data.gpsHome, data.gpsLatLon) + 540 % 360 home = math.floor(LEFT_POS + ((bearing - data.heading + (361 + HEADING_DEG / 2)) % 360) * PIXEL_DEG - 2.5) end if home >= LEFT_POS - (SMLCD and 0 or 7) and home <= RIGHT_POS - 1 then @@ -156,11 +156,11 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic lcd.drawText(LEFT_POS + 12, 42, tmp < 1000 and math.floor(tmp + 0.5) .. units[data.dist_unit] or (string.format("%.1f", tmp / (data.dist_unit == 9 and 1000 or 5280)) .. (data.dist_unit == 9 and "km" or "mi")), SMLSIZE + data.telemFlags) tmp = (not data.telem or data.cell < config[3].v or (data.showCurr and config[23].v == 0 and data.fuel <= config[17].v)) and FLASH or 0 if data.showFuel then - if config[23].v == 0 then + if config[23].v > 0 or (data.crsf and data.showMax) then + lcd.drawText(RIGHT_POS - 2, 9, (data.crsf and data.fuelRaw or data.fuel), SMLSIZE + RIGHT + tmp) + else lcd.drawText(RIGHT_POS - 7, 8, data.fuel, MIDSIZE + RIGHT + tmp) lcd.drawText(RIGHT_POS - 2, 13, "%", SMLSIZE + RIGHT + tmp) - else - lcd.drawText(RIGHT_POS - 2, 9, data.fuel, SMLSIZE + RIGHT + tmp) end end lcd.drawText(RIGHT_POS - 7, 19, string.format(config[1].v == 0 and "%.2f" or "%.1f", config[1].v == 0 and (data.showMax and data.cellMin or data.cell) or (data.showMax and data.battMin or data.batt)), MIDSIZE + RIGHT + tmp) @@ -297,12 +297,12 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic lcd.drawFilledRectangle(LEFT_POS - 7, 49, 7, 14, ERASE) tmp = (not data.telem or data.cell < config[3].v or (data.showFuel and config[23].v == 0 and data.fuel <= config[17].v)) and FLASH or 0 if data.showFuel then - if config[23].v == 0 then + if config[23].v > 0 or (data.crsf and data.showMax) then + lcd.drawText(LEFT_POS, data.showCurr and 8 or 10, (data.crsf and data.fuelRaw or data.fuel), MIDSIZE + RIGHT + tmp) + lcd.drawText(LEFT_POS, data.showCurr and 20 or 23, data.fUnit[data.crsf and 1 or config[23].v], SMLSIZE + RIGHT + tmp) + else lcd.drawText(LEFT_POS - 5, data.showCurr and 8 or 12, data.fuel, DBLSIZE + RIGHT + tmp) lcd.drawText(LEFT_POS, data.showCurr and 17 or 21, "%", SMLSIZE + RIGHT + tmp) - else - lcd.drawText(LEFT_POS, data.showCurr and 8 or 10, data.fuel, MIDSIZE + RIGHT + tmp) - lcd.drawText(LEFT_POS, data.showCurr and 20 or 23, data.fUnit[config[23].v], SMLSIZE + RIGHT + tmp) end end lcd.drawText(LEFT_POS - 5, data.showCurr and 25 or 32, string.format(config[1].v == 0 and "%.2f" or "%.1f", config[1].v == 0 and (data.showMax and data.cellMin or data.cell) or (data.showMax and data.battMin or data.batt)), DBLSIZE + RIGHT + tmp) diff --git a/src/iNav/radar.lua b/src/iNav/radar.lua index 6d3e7478..91dd141a 100644 --- a/src/iNav/radar.lua +++ b/src/iNav/radar.lua @@ -1,4 +1,4 @@ -local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcTrig, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) +local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcBearing, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) local LEFT_DIV = 36 local LEFT_POS = SMLCD and LEFT_DIV or 73 @@ -54,7 +54,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic else lcd.drawText(LEFT_POS + 2, 17, "Ptch", SMLSIZE) end - elseif data.showDir or data.headingRef < 0 then + elseif data.showDir or data.headingRef == -1 then -- Heading lcd.drawText(X_CNTR + 14 - (data.heading < 100 and 3 or 0) - (data.heading < 10 and 3 or 0), 57, math.floor(data.heading + 0.5) % 360 .. "\64", SMLSIZE + RIGHT + data.telemFlags) end @@ -66,7 +66,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic -- Radar if data.startup == 0 then tmp = data.headingRef - if data.showDir or data.headingRef < 0 then + if data.showDir or data.headingRef == -1 then lcd.drawText(LEFT_POS + 2, 33, "W", SMLSIZE) lcd.drawText(RIGHT_POS, 33, "E", SMLSIZE + RIGHT) tmp = 0 @@ -78,7 +78,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic if SMLCD and not data.armed then d = math.min(d, 18) end - local bearing = calcTrig(data.gpsHome, data.gpsLatLon, true) - tmp + local bearing = calcBearing(data.gpsHome, data.gpsLatLon) - tmp local rad1 = math.rad(bearing) cx = math.floor(math.sin(rad1) * d + 0.5) cy = math.floor(math.cos(rad1) * d + 0.5) @@ -163,12 +163,12 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic lcd.drawLine(LEFT_DIV, 8, LEFT_DIV, 63, SOLID, FORCE) tmp = (not data.telem or data.cell < config[3].v or (data.showFuel and config[23].v == 0 and data.fuel <= config[17].v)) and FLASH or 0 if data.showFuel then - if config[23].v == 0 then + if config[23].v > 0 or (data.crsf and data.showMax) then + lcd.drawText(LEFT_DIV, data.showCurr and 8 or 10, (data.crsf and data.fuelRaw or data.fuel), MIDSIZE + RIGHT + tmp) + lcd.drawText(LEFT_DIV, data.showCurr and 20 or 23, data.fUnit[data.crsf and 1 or config[23].v], SMLSIZE + RIGHT + tmp) + else lcd.drawText(LEFT_DIV - 5, data.showCurr and 8 or 12, data.fuel, DBLSIZE + RIGHT + tmp) lcd.drawText(LEFT_DIV, data.showCurr and 17 or 21, "%", SMLSIZE + RIGHT + tmp) - else - lcd.drawText(LEFT_DIV, data.showCurr and 8 or 10, data.fuel, MIDSIZE + RIGHT + tmp) - lcd.drawText(LEFT_DIV, data.showCurr and 20 or 23, data.fUnit[config[23].v], SMLSIZE + RIGHT + tmp) end end lcd.drawText(LEFT_DIV - 5, data.showCurr and 25 or 32, string.format(config[1].v == 0 and "%.2f" or "%.1f", config[1].v == 0 and (data.showMax and data.cellMin or data.cell) or (data.showMax and data.battMin or data.batt)), DBLSIZE + RIGHT + tmp) diff --git a/src/iNav/reset.lua b/src/iNav/reset.lua index 873484cc..4b1778af 100644 --- a/src/iNav/reset.lua +++ b/src/iNav/reset.lua @@ -25,7 +25,7 @@ data.speedMax = 0 data.currentMax = 0 data.battMin = 0 data.cellMin = 0 -data.rssiMin = 0 +data.rssiMin = 100 data.altMin = 0 data.altMax = data.alt_unit == 10 and 50 or 30 diff --git a/src/iNav/view.lua b/src/iNav/view.lua index bd4b1696..61d5b50e 100644 --- a/src/iNav/view.lua +++ b/src/iNav/view.lua @@ -1,4 +1,4 @@ -local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcTrig, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) +local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, icons, calcBearing, calcDir, VERSION, SMLCD, FLASH, FILE_PATH) local RIGHT_POS = SMLCD and 129 or 195 local GAUGE_WIDTH = SMLCD and 82 or 149 @@ -62,7 +62,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic if data.showHead and data.startup == 0 then if data.telem then local indicatorDisplayed = false - if data.showDir or data.headingRef < 0 or not SMLCD then + if data.showDir or data.headingRef == -1 or not SMLCD then lcd.drawText(X_CNTR_1 - 2, 9, "N " .. math.floor(data.heading + 0.5) % 360 .. "\64", SMLSIZE) lcd.drawText(X_CNTR_1 + 10, 21, "E", SMLSIZE) lcd.drawText(X_CNTR_1 - 14, 21, "W", SMLSIZE) @@ -72,7 +72,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic drawDirection(data.heading, 140, 7, X_CNTR_1, 23) indicatorDisplayed = true end - if not data.showDir or data.headingRef >= 0 or not SMLCD then + if not data.showDir or data.headingRef ~= -1 or not SMLCD then if not indicatorDisplayed or not SMLCD then drawDirection(data.heading - data.headingRef, 145, 8, SMLCD and 63 or 133, 19) end @@ -80,7 +80,7 @@ local function view(data, config, modes, units, labels, gpsDegMin, hdopGraph, ic end if data.gpsHome ~= false and data.distanceLast >= data.distRef then if not data.showDir or not SMLCD then - local bearing = calcTrig(data.gpsHome, data.gpsLatLon, true) - data.headingRef + local bearing = calcBearing(data.gpsHome, data.gpsLatLon) - data.headingRef local rad1 = math.rad(bearing) local x1 = math.floor(math.sin(rad1) * 10 + 0.5) + X_CNTR_2 local y1 = 19 - math.floor(math.cos(rad1) * 10 + 0.5)