From e3a8239b2193e006dfec10efae04fd25e87856a4 Mon Sep 17 00:00:00 2001 From: teckel12 Date: Tue, 26 Sep 2017 13:29:01 -0400 Subject: [PATCH 1/9] Reduced whitespace and comments to lower file size and possibly memory used --- .gitignore | 2 + iNav.lua | 1130 ++++++++++++++++++++++++++-------------------------- 2 files changed, 562 insertions(+), 570 deletions(-) diff --git a/.gitignore b/.gitignore index e69de29b..49260768 100644 --- a/.gitignore +++ b/.gitignore @@ -0,0 +1,2 @@ +.vscode/ +node_modules/ \ No newline at end of file diff --git a/iNav.lua b/iNav.lua index a629f8d1..41fb9021 100644 --- a/iNav.lua +++ b/iNav.lua @@ -1,570 +1,560 @@ --- Lua Telemetry Flight Status Screen for INAV/Taranis - v1.1 --- --- Author: https://github.com/teckel12 --- Docs: https://github.com/iNavFlight/LuaTelemetry - -local WAVPATH = "/SCRIPTS/TELEMETRY/iNav/" -local FLASH = INVERS + BLINK -local QX7 = (LCD_W < 212) -local TIMER_POS = (QX7) and 60 or 150 -local RXBATT_POS = LCD_W - 17 -local RIGHT_POS = (QX7) and 129 or 195 -local GAUGE_WIDTH = (QX7) and 82 or 149 -local MODE_POS = (QX7) and 48 or 90 -local X_CNTR_1 = QX7 and 67 or 70 -local X_CNTR_2 = QX7 and 67 or 140 -local X_CNTR_3 = QX7 and 67 or 110 - -local modeIdPrev = false -local armedPrev = false -local headingHoldPrev = false -local altHoldPrev = false -local gpsFixPrev = false -local gpsFix = false -local headingRef = -1 -local altNextPlay = 0 -local battNextPlay = 0 -local battPercentPlayed = 100 -local telemFlags = -1 -local battlow = false -local showMax = false -local showDir = true -local showCurr = true -local battPos1 = 49 -local battPos2 = 49 - --- modes --- t = text --- f = flags for text --- a = show alititude hold --- w = wave file -local modes = { - { t="NO TELEM", f=BLINK + INVERS, a=false, w=false }, - { t="HORIZON", f=0, a=true, w="hrznmd.wav" }, - { t="ANGLE", f=0, a=true, w="anglmd.wav" }, - { t="ACRO", f=0, a=true, w="acromd.wav" }, - { t=" NOT OK ", f=BLINK + INVERS, a=false, w=false }, - { t="READY", f=0, a=false, w="ready.wav" }, - { t="POS HOLD", f=0, a=true, w="poshld.wav" }, - { t="3D HOLD", f=0, a=true, w="3dhold.wav" }, - { t="WAYPOINT", f=0, a=false, w="waypt.wav" }, - { t=" RTH ", f=BLINK + INVERS, a=false, w="rtl.wav" }, - { t="FAILSAFE", f=BLINK + INVERS, a=false, w="fson.wav" }, -} - -local data = {} - -local function getTelemetryId(name) - field = getFieldInfo(name) - if field then - return field.id - else - return -1 - end -end - -local function flightModes() - armed = false - headFree = false - headingHold = false - altHold = false - ok2arm = false - posHold = false - if (data.telemetry) then - local modeA = math.floor(data.mode / 10000) - local modeB = math.floor(data.mode / 1000 ) % 10 - local modeC = math.floor(data.mode / 100) % 10 - local modeD = math.floor(data.mode / 10) % 10 - local modeE = data.mode % 10 - if (bit32.band(modeE, 4) > 0) then - armed = true - if (bit32.band(modeD, 2) > 0) then - modeId = 2 -- Horizon mode - elseif (bit32.band(modeD, 1) > 0) then - modeId = 3 -- Angle mode - else - modeId = 4 -- Acro mode - end - end - if (bit32.band(modeE, 2) > 0 or modeE == 0) then - modeId = 5 -- Not OK to fly - else - ok2arm = true - if (not armed) then - modeId = 6 -- Ready to fly - end - end - if (bit32.band(modeB, 4) > 0) then - headFree = true - end - if (bit32.band(modeC, 4) > 0) then - if (armed) then - modeId = 7 -- Position hold - posHold = true - end - end - if (bit32.band(modeC, 2) > 0) then - altHold = true - if (posHold) then - modeId = 8 -- 3D potition hold - end - end - if (bit32.band(modeC, 1) > 0) then - headingHold = true - end - if (bit32.band(modeB, 2) > 0) then - modeId = 9 -- Waypoint - end - if (bit32.band(modeB, 4) > 0) then - modeId = 10 -- Return to home - end - if (bit32.band(modeA, 4) > 0) then - modeId = 11 -- Failsafe - end - else - modeId = 1 -- No telemetry - end - - -- *** Audio feedback on flight modes *** - local vibrate = false - local beep = false - if (armed and not armedPrev) then - data.timerStart = getTime() - data.distLastPositive = 0 - headingRef = data.heading - data.gpsHome = false - battPercentPlayed = 100 - battlow = false - showMax = false - showDir = false - playFile(WAVPATH .. "engarm.wav") - elseif (not armed and armedPrev) then - if (data.distLastPositive < 15) then - headingRef = -1 - showDir = true - end - playFile(WAVPATH .. "engdrm.wav") - end - if (gpsFix and not gpsFixPrev) then - playFile(WAVPATH .. "gps.wav") - playFile(WAVPATH .. "good.wav") - elseif (not gpsFix and gpsFixPrev) then - playFile(WAVPATH .. "gps.wav") - playFile(WAVPATH .. "lost.wav") - end - if (modeIdPrev and modeIdPrev ~= modeId) then - if (not armed and modeId == 6 and modeIdPrev == 5) then - playFile(WAVPATH .. modes[modeId].w) - end - if (armed) then - if (modes[modeId].w) then - playFile(WAVPATH .. modes[modeId].w) - end - if (modes[modeId].f > 0) then - vibrate = true - end - end - end - if (armed) then - if (altHold and modes[modeId].a and altHoldPrev ~= altHold) then - playFile(WAVPATH .. "althld.wav") - playFile(WAVPATH .. "active.wav") - elseif (not altHold and modes[modeId].a and altHoldPrev ~= altHold) then - playFile(WAVPATH .. "althld.wav") - playFile(WAVPATH .. "off.wav") - end - if (headingHold and headingHoldPrev ~= headingHold) then - playFile(WAVPATH .. "hedhlda.wav") - elseif (not headingHold and headingHoldPrev ~= headingHold) then - playFile(WAVPATH .. "hedhld.wav") - playFile(WAVPATH .. "off.wav") - end - if (data.altitude > 400) then - if (getTime() > altNextPlay) then - playNumber(data.altitude, 10) - altNextPlay = getTime() + 1000 - else - beep = true - end - end - if (battPercentPlayed > data.fuel) then - if (data.fuel == 30 or data.fuel == 25) then - playFile(WAVPATH .. "batlow.wav") - playNumber(data.fuel, 13) - battPercentPlayed = data.fuel - elseif (data.fuel % 10 == 0 and data.fuel < 100 and data.fuel >= 40) then - playFile(WAVPATH .. "battry.wav") - playNumber(data.fuel, 13) - battPercentPlayed = data.fuel - end - end - if (data.fuel <= 20 or data.cell < 3.40) then - if (getTime() > battNextPlay) then - playFile(WAVPATH .. "batcrt.wav") - if (data.fuel <= 20 and battPercentPlayed > data.fuel) then - playNumber(data.fuel, 13) - battPercentPlayed = data.fuel - end - battNextPlay = getTime() + 500 - else - vibrate = true - beep = true - end - battlow = true - elseif (data.cell < 3.50) then - if (not battlow) then - playFile(WAVPATH .. "batlow.wav") - battlow = true - end - else - battNextPlay = 0 - end - if (headFree or modes[modeId].f > 0) then - beep = true - end - if (data.rssi < data.rssiLow) then - if (data.rssi < data.rssiCrit) then - vibrate = true - end - beep = true - end - if (vibrate) then - playHaptic(25, 3000) - end - if (beep) then - playTone(2000, 100, 3000, PLAY_NOW) - end - else - battlow = false - battPercentPlayed = 100 - end - modeIdPrev = modeId - headingHoldPrev = headingHold - altHoldPrev = altHold - armedPrev = armed - gpsFixPrev = gpsFix -end - -local function init() - local rssi, low, crit = getRSSI() - local ver, radio, maj, minor, rev = getVersion() - local general = getGeneralSettings() - data.rssiLow = low - data.rssiCrit = crit - data.version = maj + minor / 10 -- Make sure OpenTX 2.2+ - data.txBattMin = general["battMin"] - data.txBattMax = general["battMax"] - data.units = general["imperial"] -- 0 = metric / 1 = imperial - data.modelName = model.getInfo()["name"] - data.mode_id = getTelemetryId("Tmp1") - data.rxBatt_id = getTelemetryId("RxBt") - data.satellites_id = getTelemetryId("Tmp2") - data.gpsAlt_id = getTelemetryId("GAlt") - data.gpsLatLon_id = getTelemetryId("GPS") - data.heading_id = getTelemetryId("Hdg") - data.altitude_id = getTelemetryId("Alt") - data.distance_id = getTelemetryId("Dist") - data.speed_id = getTelemetryId("GSpd") - data.current_id = getTelemetryId("Curr") - data.altitudeMax_id = getTelemetryId("Alt+") - data.distanceMax_id = getTelemetryId("Dist+") - data.speedMax_id = getTelemetryId("GSpd+") - data.currentMax_id = getTelemetryId("Curr+") - data.batt_id = getTelemetryId("VFAS") - data.battMin_id = getTelemetryId("VFAS-") - data.fuel_id = getTelemetryId("Fuel") - data.rssi_id = getTelemetryId("RSSI") - data.rssiMin_id = getTelemetryId("RSSI-") - data.txBatt_id = getTelemetryId("tx-voltage") - data.ras_id = getTelemetryId("RAS") - data.timerStart = 0 - data.timer = 0 - data.distLastPositive = 0 - data.gpsHome = false - if (data.current_id == -1 or data.fuel_id == -1) then - showCurr = false - data.current = 0 - data.currentMax = 0 - data.fuel = 100 - battPos1 = 45 - battPos2 = 41 - end -end - -local function background() - data.rssi = getValue(data.rssi_id) - if (data.rssi > 0 or telemFlags < 0) then - data.telemetry = true - data.mode = getValue(data.mode_id) - data.rxBatt = getValue(data.rxBatt_id) - data.satellites = getValue(data.satellites_id) - data.gpsAlt = getValue(data.gpsAlt_id) - data.gpsLatLon = getValue(data.gpsLatLon_id) - data.heading = getValue(data.heading_id) - data.altitude = getValue(data.altitude_id) - data.distance = math.floor(getValue(data.distance_id) * 3.28084 + 0.5) - data.speed = getValue(data.speed_id) - if (showCurr) then - data.current = getValue(data.current_id) - data.currentMax = getValue(data.currentMax_id) - data.fuel = getValue(data.fuel_id) - end - data.altitudeMax = getValue(data.altitudeMax_id) - data.distanceMax = getValue(data.distanceMax_id) - data.speedMax = getValue(data.speedMax_id) - data.batt = getValue(data.batt_id) - data.battMin = getValue(data.battMin_id) - data.cells = math.floor(data.batt / 4.3) + 1 - data.cell = data.batt / data.cells - data.cellMin = data.battMin / data.cells - data.rssiMin = getValue(data.rssiMin_id) - data.txBatt = getValue(data.txBatt_id) - data.rssiLast = data.rssi - -- *** Stuff for testing *** - --data.distance = 237 - --data.gpsLatLon["lat"] = math.deg(data.gpsLatLon["lat"]) - --data.gpsLatLon["lon"] = math.deg(data.gpsLatLon["lon"] * 2.2) - if (data.distance > 0) then - data.distLastPositive = data.distance - end - gpsFix = (type(data.gpsLatLon) == "table" and data.satellites > 3900) - telemFlags = 0 - else - data.telemetry = false - telemFlags = FLASH - end - - flightModes() - - if (armed and gpsFix and type(data.gpsLatLon) == "table" and type(data.gpsHome) ~= "table") then - data.gpsHome = data.gpsLatLon - end -end - -local function drawDirection(headingDisplay, width, size, centerx, centery) - local rad1 = math.rad(headingDisplay) - local rad2 = math.rad(headingDisplay + width) - local rad3 = math.rad(headingDisplay - width) - local x1 = math.floor(math.sin(rad1) * size + 0.5) + centerx - local y1 = centery - math.floor(math.cos(rad1) * size + 0.5) - local x2 = math.floor(math.sin(rad2) * size + 0.5) + centerx - local y2 = centery - math.floor(math.cos(rad2) * size + 0.5) - local x3 = math.floor(math.sin(rad3) * size + 0.5) + centerx - local y3 = centery - math.floor(math.cos(rad3) * size + 0.5) - lcd.drawLine(x1, y1, x2, y2, SOLID, FORCE) - lcd.drawLine(x1, y1, x3, y3, SOLID, FORCE) - if (headingHold and armed) then - lcd.drawFilledRectangle((x2 + x3) / 2 - 1.5, (y2 + y3) / 2 - 1.5, 4, 4, SOLID) - else - lcd.drawLine(x2, y2, x3, y3, DOTTED, FORCE) - end -end - -local function run(event) - lcd.clear() - background() - - -- *** Minimum OpenTX version *** - if (data.version < 2.2) then - lcd.drawText(QX7 and 5 or 47, 27, "OpenTX v2.2.0+ Required") - return - end - - -- *** Title *** - if (armed) then - --data.timer = model.getTimer(0)["value"] -- Show timer1 instead of custom timer - data.timer = (getTime() - data.timerStart) / 100 - end - lcd.drawFilledRectangle(0, 0, LCD_W, 8) - lcd.drawText(0, 0, data.modelName, INVERS) - lcd.drawTimer(TIMER_POS, 1, data.timer, SMLSIZE + INVERS) - lcd.drawFilledRectangle(86, 1, 19, 6, ERASE) - lcd.drawLine(105, 2, 105, 5, SOLID, ERASE) - local battGauge = math.max(math.min((data.txBatt - data.txBattMin) / (data.txBattMax - data.txBattMin) * 17, 17), 0) + 86 - for i = 87, battGauge, 2 do - lcd.drawLine(i, 2, i, 5, SOLID, FORCE) - end - if (not QX7) then - lcd.drawNumber(110 , 1, data.txBatt * 10.05, SMLSIZE + PREC1 + INVERS) - lcd.drawText(lcd.getLastPos(), 1, "V", SMLSIZE + INVERS) - end - if (data.rxBatt > 0 and data.telemetry) then - lcd.drawNumber(RXBATT_POS, 1, data.rxBatt * 10.05, SMLSIZE + PREC1 + INVERS) - lcd.drawText(lcd.getLastPos(), 1, "V", SMLSIZE + INVERS) - end - - -- *** GPS Coords *** - if (type(data.gpsLatLon) == "table") then - gpsFlags = (telemFlags > 0 or not gpsFix) and FLASH or 0 - value = math.floor(data.gpsAlt + 0.5) .. "ft" - lcd.drawText(85, 9, value, SMLSIZE) - pos = 85 + (RIGHT_POS - lcd.getLastPos()) - lcd.drawText(pos, 17, value, SMLSIZE + gpsFlags) - - value = string.format("%.4f", data.gpsLatLon["lat"]) - lcd.drawText(85, 9, value, SMLSIZE) - pos = 85 + (RIGHT_POS - lcd.getLastPos()) - lcd.drawText(pos, 25, value, SMLSIZE + gpsFlags) - - value = string.format("%.4f", data.gpsLatLon["lon"]) - lcd.drawText(85, 9, value, SMLSIZE) - pos = 85 + (RIGHT_POS - lcd.getLastPos()) - lcd.drawText(pos, 33, value, SMLSIZE + gpsFlags) - else - lcd.drawFilledRectangle(RIGHT_POS - 41, 17, 41, 23, INVERS) - lcd.drawText(RIGHT_POS - 37, 20, "No GPS", INVERS) - lcd.drawText(RIGHT_POS - 28, 30, "Fix", INVERS) - end - - -- *** Satellites *** - value = "Sats " .. tonumber(string.sub(data.satellites, -2)) - lcd.drawText(85, 9, value, SMLSIZE) - pos = 85 + (RIGHT_POS - lcd.getLastPos()) - lcd.drawText(85, 9, " ", SMLSIZE) - lcd.drawText(pos, 9, value, SMLSIZE + telemFlags) - - -- *** Directional indicator *** - if (event == EVT_ROT_LEFT or event == EVT_ROT_RIGHT or event == EVT_PLUS_BREAK or event == EVT_MINUS_BREAK) then - showDir = not showDir - end - if (data.telemetry) then - local indicatorDisplayed = false - if (showDir or headingRef < 0 or not QX7) then - lcd.drawText(X_CNTR_1 - 2, 9, "N " .. math.floor(data.heading + 0.5) .. "\64", SMLSIZE) - lcd.drawText(X_CNTR_1 + 10, 21, "E", SMLSIZE) - lcd.drawText(X_CNTR_1 - 14, 21, "W", SMLSIZE) - drawDirection(data.heading, 135, 7, X_CNTR_1, 23) - indicatorDisplayed = true - end - if (not showDir or headingRef >= 0 or not QX7) then - if (not indicatorDisplayed or not QX7) then - drawDirection(data.heading - headingRef, 145, 10, X_CNTR_2, 19) - end - end - end - if (type(data.gpsLatLon) == "table" and type(data.gpsHome) == "table" and data.distLastPositive >= 25) then - if (not showDir or not QX7) then - local o1 = math.rad(data.gpsHome["lat"]) - local a1 = math.rad(data.gpsHome["lon"]) - local o2 = math.rad(data.gpsLatLon["lat"]) - local a2 = math.rad(data.gpsLatLon["lon"]) - local y = math.sin(a2 - a1) * math.cos(o2) - local x = (math.cos(o1) * math.sin(o2)) - (math.sin(o1) * math.cos(o2) * math.cos(a2 - a1)) - local bearing = math.deg(math.atan2(y, x)) - headingRef - local rad1 = math.rad(bearing) - local x1 = math.floor(math.sin(rad1) * 10 + 0.5) + X_CNTR_3 - local y1 = 19 - math.floor(math.cos(rad1) * 10 + 0.5) - lcd.drawLine(X_CNTR_3, 19, x1, y1, DOTTED, FORCE) - lcd.drawFilledRectangle(x1 - 1, y1 - 1, 3, 3, ERASE) - lcd.drawFilledRectangle(x1 - 1, y1 - 1, 3, 3, SOLID) - end - end - - -- *** Head free warning *** - if (armed and headFree) then - lcd.drawText(85, 9, "HF", SMLSIZE + FLASH) - end - - -- *** Display flight mode (centered) *** - lcd.drawText(48, 34, modes[modeId].t, SMLSIZE + modes[modeId].f) - pos = MODE_POS + (87 - lcd.getLastPos()) / 2 - lcd.drawFilledRectangle(46, 33, 40, 10, ERASE) - lcd.drawText(pos, 33, modes[modeId].t, SMLSIZE + modes[modeId].f) - - -- *** Data *** - if (not armed) then - if (event == EVT_ROT_LEFT or event == EVT_ROT_RIGHT or event == EVT_PLUS_BREAK or event == EVT_MINUS_BREAK) then - showMax = not showMax - end - end - if (showMax) then - altd = data.altitudeMax - dist = math.floor(data.distanceMax * 3.28084 + 0.5) - sped = data.speedMax - curr = data.currentMax - batt = data.battMin - rssi = data.rssiMin - lcd.drawText(0, 9, "Alt", SMLSIZE) - lcd.drawText(15, 9, "\192", SMLSIZE) - lcd.drawText(0, 17, "Dst\192", SMLSIZE) - lcd.drawText(0, 25, "Spd\192", SMLSIZE) - if (showCurr) then - lcd.drawText(0, 33, "Cur\192", SMLSIZE) - end - lcd.drawText(0, battPos1, "Bat\193", SMLSIZE) - lcd.drawText(0, 57, "RSI", SMLSIZE) - lcd.drawText(15, 57, "\193", SMLSIZE) - else - altd = data.altitude - dist = data.distLastPositive - sped = data.speed - curr = data.current - batt = data.batt - rssi = data.rssiLast - lcd.drawText(0, 9, "Altd ", SMLSIZE) - lcd.drawText(0, 17, "Dist", SMLSIZE) - lcd.drawText(0, 25, "Sped", SMLSIZE) - if (showCurr) then - lcd.drawText(0, 33, "Curr", SMLSIZE) - end - lcd.drawText(0, battPos1, "Batt", SMLSIZE) - lcd.drawText(0, 57, "RSSI", SMLSIZE) - end - lcd.drawText(22, 9, math.floor(altd + 0.5), SMLSIZE + telemFlags) - if (altd < 1000) then - lcd.drawText(lcd.getLastPos(), 9, "ft", SMLSIZE + telemFlags) - end - if (armed and altHold and modes[modeId].a) then - lcd.drawText(lcd.getLastPos() + 1, 9, "\192", SMLSIZE + INVERS) -- Altitude hold notification - end - lcd.drawText(22, 17, dist, SMLSIZE + telemFlags) - if (dist < 1000) then - lcd.drawText(lcd.getLastPos(), 17, "ft", SMLSIZE + telemFlags) - end - lcd.drawText(22, 25, math.floor(sped + 0.5), SMLSIZE + telemFlags) - if (sped < 100) then - lcd.drawText(lcd.getLastPos(), 25, "mph", SMLSIZE + telemFlags) - end - battFlags = (telemFlags > 0 or battlow) and FLASH or 0 - if (showCurr) then - lcd.drawNumber(22, 33, curr * 10.05, SMLSIZE + PREC1 + telemFlags) - if (curr < 100) then - lcd.drawText(lcd.getLastPos(), 33, "A", SMLSIZE + telemFlags) - end - lcd.drawText(0, 41, "Fuel", SMLSIZE) - lcd.drawText(22, 41, data.fuel .. "%", SMLSIZE + battFlags) - end - lcd.drawNumber(22, battPos1, batt * 10.05, SMLSIZE + PREC1 + battFlags) - lcd.drawText(lcd.getLastPos(), battPos1, "V", SMLSIZE + battFlags) - rssiFlags = (telemFlags > 0 or data.rssi < data.rssiLow) and FLASH or 0 - lcd.drawText(22, 57, rssi .. "dB", SMLSIZE + rssiFlags) - - -- *** Bar graphs *** - if (showCurr) then - lcd.drawGauge(46, 41, GAUGE_WIDTH, 7, math.min(data.fuel, 98), 100) - if (data.fuel == 0) then - lcd.drawLine(47, 42, 47, 46, SOLID, ERASE) - end - end - lcd.drawGauge(46, battPos2, GAUGE_WIDTH, 56 - battPos2, math.min(math.max(data.cell - 3.3, 0) * 111.1, 98), 100) - min = (GAUGE_WIDTH - 2) * (math.min(math.max(data.cellMin - 3.3, 0) * 111.1, 99) / 100) + 47 - lcd.drawLine(min, battPos2 + 1, min, 54, SOLID, ERASE) - local rssiGauge = math.max(math.min((data.rssiLast - data.rssiCrit) / (100 - data.rssiCrit) * 100, 98), 0) - lcd.drawGauge(46, 57, GAUGE_WIDTH, 7, rssiGauge, 100) - min = (GAUGE_WIDTH - 2) * (math.max(math.min((data.rssiMin - data.rssiCrit) / (100 - data.rssiCrit) * 100, 99), 0) / 100) + 47 - lcd.drawLine(min, 58, min, 62, SOLID, ERASE) - - -- *** Altitude graph for X9D *** - if (not QX7) then - lcd.drawRectangle(199, 9, 13, 48, SOLID) - height = math.max(math.min(math.ceil(data.altitude / 400 * 46), 46), 0) - lcd.drawFilledRectangle(200, 56 - height, 11, height, INVERS) - local max = 56 - math.max(math.min(math.ceil(data.altitudeMax / 400 * 46), 46), 0) - lcd.drawLine(200, max, 210, max, DOTTED, FORCE) - lcd.drawText(199, 58, "Alt", SMLSIZE) - end - - return 1 -end - -return {init=init, run=run, background=background} \ No newline at end of file +-- Lua Telemetry Flight Status Screen for INAV/Taranis-v1.1 +-- +-- Author: https://github.com/teckel12 +-- Docs: https://github.com/iNavFlight/LuaTelemetry + +local WAVPATH="/SCRIPTS/TELEMETRY/iNav/" +local FLASH=INVERS+BLINK +local QX7=(LCD_W<212) +local TIMER_POS=(QX7) and 60 or 150 +local RXBATT_POS=LCD_W-17 +local RIGHT_POS=(QX7) and 129 or 195 +local GAUGE_WIDTH=(QX7) and 82 or 149 +local MODE_POS=(QX7) and 48 or 90 +local X_CNTR_1=QX7 and 67 or 70 +local X_CNTR_2=QX7 and 67 or 140 +local X_CNTR_3=QX7 and 67 or 110 + +local modeIdPrev=false +local armedPrev=false +local headingHoldPrev=false +local altHoldPrev=false +local gpsFixPrev=false +local gpsFix=false +local headingRef=-1 +local altNextPlay=0 +local battNextPlay=0 +local battPercentPlayed=100 +local telemFlags=-1 +local battlow=false +local showMax=false +local showDir=true +local showCurr=true +local battPos1=49 +local battPos2=49 + +local modes={ +{t="NO TELEM",f=FLASH,a=false,w=false}, +{t="HORIZON",f=0,a=true,w="hrznmd.wav"}, +{t="ANGLE",f=0,a=true,w="anglmd.wav"}, +{t="ACRO",f=0,a=true,w="acromd.wav"}, +{t=" NOT OK ",f=FLASH,a=false,w=false}, +{t="READY",f=0,a=false,w="ready.wav"}, +{t="POS HOLD",f=0,a=true,w="poshld.wav"}, +{t="3D HOLD",f=0,a=true,w="3dhold.wav"}, +{t="WAYPOINT",f=0,a=false,w="waypt.wav"}, +{t=" RTH ",f=FLASH,a=false,w="rtl.wav"}, +{t="FAILSAFE",f=FLASH,a=false,w="fson.wav"} +} + +local data={} + +local function getTelemetryId(name) + local field=getFieldInfo(name) + if field then + return field.id + else + return -1 + end +end + +local function flightModes() + armed=false + headFree=false + headingHold=false + altHold=false + ok2arm=false + posHold=false + if data.telemetry then + local modeA=math.floor(data.mode/10000) + local modeB=math.floor(data.mode/1000)%10 + local modeC=math.floor(data.mode/100)%10 + local modeD=math.floor(data.mode/10)%10 + local modeE=data.mode%10 + if bit32.band(modeE,4)>0 then + armed=true + if bit32.band(modeD,2)>0 then + modeId=2 + elseif bit32.band(modeD,1)>0 then + modeId=3 + else + modeId=4 + end + end + if bit32.band(modeE,2)>0 or modeE==0 then + modeId=5 + else + ok2arm=true + if not armed then + modeId=6 + end + end + if bit32.band(modeB,4)>0 then + headFree=true + end + if bit32.band(modeC,4)>0 then + if armed then + modeId=7 + posHold=true + end + end + if bit32.band(modeC,2)>0 then + altHold=true + if posHold then + modeId=8 + end + end + if bit32.band(modeC,1)>0 then + headingHold=true + end + if bit32.band(modeB,2)>0 then + modeId=9 + end + if bit32.band(modeB,4)>0 then + modeId=10 + end + if bit32.band(modeA,4)>0 then + modeId=11 + end + else + modeId=1 + end + + -- Audio flight mode feedback + local vibrate=false + local beep=false + if armed and not armedPrev then + data.timerStart=getTime() + data.distLastPositive=0 + headingRef=data.heading + data.gpsHome=false + battPercentPlayed=100 + battlow=false + showMax=false + showDir=false + playFile(WAVPATH.."engarm.wav") + elseif not armed and armedPrev then + if data.distLastPositive<15 then + headingRef=-1 + showDir=true + end + playFile(WAVPATH.."engdrm.wav") + end + if gpsFix and not gpsFixPrev then + playFile(WAVPATH.."gps.wav") + playFile(WAVPATH.."good.wav") + elseif not gpsFix and gpsFixPrev then + playFile(WAVPATH.."gps.wav") + playFile(WAVPATH.."lost.wav") + end + if modeIdPrev and modeIdPrev~=modeId then + if not armed and modeId==6 and modeIdPrev==5 then + playFile(WAVPATH..modes[modeId].w) + end + if armed then + if modes[modeId].w then + playFile(WAVPATH..modes[modeId].w) + end + if modes[modeId].f>0 then + vibrate=true + end + end + end + if armed then + if altHold and modes[modeId].a and altHoldPrev~=altHold then + playFile(WAVPATH.."althld.wav") + playFile(WAVPATH.."active.wav") + elseif not altHold and modes[modeId].a and altHoldPrev~=altHold then + playFile(WAVPATH.."althld.wav") + playFile(WAVPATH.."off.wav") + end + if headingHold and headingHoldPrev~=headingHold then + playFile(WAVPATH.."hedhlda.wav") + elseif not headingHold and headingHoldPrev~=headingHold then + playFile(WAVPATH.."hedhld.wav") + playFile(WAVPATH.."off.wav") + end + if data.altitude>400 then + if getTime()>altNextPlay then + playNumber(data.altitude,10) + altNextPlay=getTime()+1000 + else + beep=true + end + end + if battPercentPlayed>data.fuel then + if data.fuel==30 or data.fuel==25 then + playFile(WAVPATH.."batlow.wav") + playNumber(data.fuel,13) + battPercentPlayed=data.fuel + elseif data.fuel%10==0 and data.fuel<100 and data.fuel>=40 then + playFile(WAVPATH.."battry.wav") + playNumber(data.fuel,13) + battPercentPlayed=data.fuel + end + end + if data.fuel<=20 or data.cell<3.40 then + if getTime()>battNextPlay then + playFile(WAVPATH.."batcrt.wav") + if data.fuel<=20 and battPercentPlayed>data.fuel then + playNumber(data.fuel,13) + battPercentPlayed=data.fuel + end + battNextPlay=getTime()+500 + else + vibrate=true + beep=true + end + battlow=true + elseif data.cell<3.50 then + if not battlow then + playFile(WAVPATH.."batlow.wav") + battlow=true + end + else + battNextPlay=0 + end + if headFree or modes[modeId].f>0 then + beep=true + end + if data.rssi0 or telemFlags<0 then + data.telemetry=true + data.mode=getValue(data.mode_id) + data.rxBatt=getValue(data.rxBatt_id) + data.satellites=getValue(data.satellites_id) + data.gpsAlt=getValue(data.gpsAlt_id) + data.gpsLatLon=getValue(data.gpsLatLon_id) + data.heading=getValue(data.heading_id) + data.altitude=getValue(data.altitude_id) + data.distance=math.floor(getValue(data.distance_id)*3.28084+0.5) + data.speed=getValue(data.speed_id) + if showCurr then + data.current=getValue(data.current_id) + data.currentMax=getValue(data.currentMax_id) + data.fuel=getValue(data.fuel_id) + end + data.altitudeMax=getValue(data.altitudeMax_id) + data.distanceMax=getValue(data.distanceMax_id) + data.speedMax=getValue(data.speedMax_id) + data.batt=getValue(data.batt_id) + data.battMin=getValue(data.battMin_id) + data.cells=math.floor(data.batt/4.3)+1 + data.cell=data.batt/data.cells + data.cellMin=data.battMin/data.cells + data.rssiMin=getValue(data.rssiMin_id) + data.txBatt=getValue(data.txBatt_id) + data.rssiLast=data.rssi + --data.distance=237 + --data.gpsLatLon["lat"]=math.deg(data.gpsLatLon["lat"]) + --data.gpsLatLon["lon"]=math.deg(data.gpsLatLon["lon"]*2.2) + if data.distance>0 then + data.distLastPositive=data.distance + end + gpsFix=(type(data.gpsLatLon)=="table" and data.satellites>3900) + telemFlags=0 + else + data.telemetry=false + telemFlags=FLASH + end + + flightModes() + + if armed and gpsFix and type(data.gpsLatLon)=="table" and type(data.gpsHome)~="table" then + data.gpsHome=data.gpsLatLon + end +end + +local function drawDirection(headingDisplay,width,size,centerx,centery) + local rad1=math.rad(headingDisplay) + local rad2=math.rad(headingDisplay+width) + local rad3=math.rad(headingDisplay-width) + local x1=math.floor(math.sin(rad1)*size+0.5)+centerx + local y1=centery-math.floor(math.cos(rad1)*size+0.5) + local x2=math.floor(math.sin(rad2)*size+0.5)+centerx + local y2=centery-math.floor(math.cos(rad2)*size+0.5) + local x3=math.floor(math.sin(rad3)*size+0.5)+centerx + local y3=centery-math.floor(math.cos(rad3)*size+0.5) + lcd.drawLine(x1,y1,x2,y2,SOLID,FORCE) + lcd.drawLine(x1,y1,x3,y3,SOLID,FORCE) + if headingHold and armed then + lcd.drawFilledRectangle((x2+x3)/2-1.5,(y2+y3)/2-1.5,4,4,SOLID) + else + lcd.drawLine(x2,y2,x3,y3,DOTTED,FORCE) + end +end + +local function run(event) + lcd.clear() + background() + + if data.version<2.2 then + lcd.drawText(QX7 and 5 or 47,27,"OpenTX v2.2.0+ Required") + return + end + + -- Title + if armed then + data.timer=(getTime()-data.timerStart)/100 + end + lcd.drawFilledRectangle(0,0,LCD_W,8) + lcd.drawText(0,0,data.modelName,INVERS) + lcd.drawTimer(TIMER_POS,1,data.timer,SMLSIZE+INVERS) + lcd.drawFilledRectangle(86,1,19,6,ERASE) + lcd.drawLine(105,2,105,5,SOLID,ERASE) + local battGauge=math.max(math.min((data.txBatt-data.txBattMin)/(data.txBattMax-data.txBattMin)*17,17),0)+86 + for i=87,battGauge,2 do + lcd.drawLine(i,2,i,5,SOLID,FORCE) + end + if not QX7 then + lcd.drawNumber(110 ,1,data.txBatt*10.05,SMLSIZE+PREC1+INVERS) + lcd.drawText(lcd.getLastPos(),1,"V",SMLSIZE+INVERS) + end + if data.rxBatt>0 and data.telemetry then + lcd.drawNumber(RXBATT_POS,1,data.rxBatt*10.05,SMLSIZE+PREC1+INVERS) + lcd.drawText(lcd.getLastPos(),1,"V",SMLSIZE+INVERS) + end + + -- GPS Coords + if type(data.gpsLatLon)=="table" then + gpsFlags=(telemFlags>0 or not gpsFix) and FLASH or 0 + value=math.floor(data.gpsAlt+0.5).."ft" + lcd.drawText(85,9,value,SMLSIZE) + pos=85+(RIGHT_POS-lcd.getLastPos()) + lcd.drawText(pos,17,value,SMLSIZE+gpsFlags) + + value=string.format("%.4f",data.gpsLatLon["lat"]) + lcd.drawText(85,9,value,SMLSIZE) + pos=85+(RIGHT_POS-lcd.getLastPos()) + lcd.drawText(pos,25,value,SMLSIZE+gpsFlags) + + value=string.format("%.4f",data.gpsLatLon["lon"]) + lcd.drawText(85,9,value,SMLSIZE) + pos=85+(RIGHT_POS-lcd.getLastPos()) + lcd.drawText(pos,33,value,SMLSIZE+gpsFlags) + else + lcd.drawFilledRectangle(RIGHT_POS-41,17,41,23,INVERS) + lcd.drawText(RIGHT_POS-37,20,"No GPS",INVERS) + lcd.drawText(RIGHT_POS-28,30,"Fix",INVERS) + end + + -- Satellites + value="Sats "..tonumber(string.sub(data.satellites,-2)) + lcd.drawText(85,9,value,SMLSIZE) + pos=85+(RIGHT_POS-lcd.getLastPos()) + lcd.drawText(85,9," ",SMLSIZE) + lcd.drawText(pos,9,value,SMLSIZE+telemFlags) + + -- Directional indicator + if event==EVT_ROT_LEFT or event==EVT_ROT_RIGHT or event==EVT_PLUS_BREAK or event==EVT_MINUS_BREAK then + showDir=not showDir + end + if data.telemetry then + local indicatorDisplayed=false + if showDir or headingRef<0 or not QX7 then + lcd.drawText(X_CNTR_1-2,9,"N "..math.floor(data.heading+0.5).."\64",SMLSIZE) + lcd.drawText(X_CNTR_1+10,21,"E",SMLSIZE) + lcd.drawText(X_CNTR_1-14,21,"W",SMLSIZE) + drawDirection(data.heading,135,7,X_CNTR_1,23) + indicatorDisplayed=true + end + if not showDir or headingRef>=0 or not QX7 then + if not indicatorDisplayed or not QX7 then + drawDirection(data.heading-headingRef,145,10,X_CNTR_2,19) + end + end + end + if type(data.gpsLatLon)=="table" and type(data.gpsHome)=="table" and data.distLastPositive >= 25 then + if not showDir or not QX7 then + local o1=math.rad(data.gpsHome["lat"]) + local a1=math.rad(data.gpsHome["lon"]) + local o2=math.rad(data.gpsLatLon["lat"]) + local a2=math.rad(data.gpsLatLon["lon"]) + local y=math.sin(a2-a1)*math.cos(o2) + local x=(math.cos(o1)*math.sin(o2))-(math.sin(o1)*math.cos(o2)*math.cos(a2-a1)) + local bearing=math.deg(math.atan2(y,x))-headingRef + local rad1=math.rad(bearing) + local x1=math.floor(math.sin(rad1)*10+0.5)+X_CNTR_3 + local y1=19-math.floor(math.cos(rad1)*10+0.5) + lcd.drawLine(X_CNTR_3,19,x1,y1,DOTTED,FORCE) + lcd.drawFilledRectangle(x1-1,y1-1,3,3,ERASE) + lcd.drawFilledRectangle(x1-1,y1-1,3,3,SOLID) + end + end + + -- Head free warning + if armed and headFree then + lcd.drawText(85,9,"HF",SMLSIZE+FLASH) + end + + -- Display flight mode + lcd.drawText(48,34,modes[modeId].t,SMLSIZE+modes[modeId].f) + pos=MODE_POS+(87-lcd.getLastPos())/2 + lcd.drawFilledRectangle(46,33,40,10,ERASE) + lcd.drawText(pos,33,modes[modeId].t,SMLSIZE+modes[modeId].f) + + -- Data + if not armed then + if event==EVT_ROT_LEFT or event==EVT_ROT_RIGHT or event==EVT_PLUS_BREAK or event==EVT_MINUS_BREAK then + showMax=not showMax + end + end + if showMax then + altd=data.altitudeMax + dist=math.floor(data.distanceMax*3.28084+0.5) + sped=data.speedMax + curr=data.currentMax + batt=data.battMin + rssi=data.rssiMin + lcd.drawText(0, 9,"Alt",SMLSIZE) + lcd.drawText(15,9,"\192",SMLSIZE) + lcd.drawText(0,17,"Dst\192",SMLSIZE) + lcd.drawText(0,25,"Spd\192",SMLSIZE) + if showCurr then + lcd.drawText(0,33,"Cur\192",SMLSIZE) + end + lcd.drawText(0,battPos1,"Bat\193",SMLSIZE) + lcd.drawText(0,57,"RSI",SMLSIZE) + lcd.drawText(15,57,"\193",SMLSIZE) + else + altd=data.altitude + dist=data.distLastPositive + sped=data.speed + curr=data.current + batt=data.batt + rssi=data.rssiLast + lcd.drawText(0, 9,"Altd ",SMLSIZE) + lcd.drawText(0,17,"Dist",SMLSIZE) + lcd.drawText(0,25,"Sped",SMLSIZE) + if showCurr then + lcd.drawText(0,33,"Curr",SMLSIZE) + end + lcd.drawText(0,battPos1,"Batt",SMLSIZE) + lcd.drawText(0,57,"RSSI",SMLSIZE) + end + lcd.drawText(22,9,math.floor(altd+0.5),SMLSIZE+telemFlags) + if altd<1000 then + lcd.drawText(lcd.getLastPos(),9,"ft",SMLSIZE+telemFlags) + end + if armed and altHold and modes[modeId].a then + lcd.drawText(lcd.getLastPos()+1,9,"\192",SMLSIZE+INVERS) + end + lcd.drawText(22,17,dist,SMLSIZE+telemFlags) + if dist<1000 then + lcd.drawText(lcd.getLastPos(),17,"ft",SMLSIZE+telemFlags) + end + lcd.drawText(22,25,math.floor(sped+0.5),SMLSIZE+telemFlags) + if sped<100 then + lcd.drawText(lcd.getLastPos(),25,"mph",SMLSIZE+telemFlags) + end + battFlags=(telemFlags>0 or battlow) and FLASH or 0 + if showCurr then + lcd.drawNumber(22,33,curr*10.05,SMLSIZE+PREC1+telemFlags) + if curr<100 then + lcd.drawText(lcd.getLastPos(),33,"A",SMLSIZE+telemFlags) + end + lcd.drawText(0,41,"Fuel",SMLSIZE) + lcd.drawText(22,41,data.fuel.."%",SMLSIZE+battFlags) + end + lcd.drawNumber(22,battPos1,batt*10.05,SMLSIZE+PREC1+battFlags) + lcd.drawText(lcd.getLastPos(),battPos1,"V",SMLSIZE+battFlags) + rssiFlags=(telemFlags>0 or data.rssi Date: Wed, 27 Sep 2017 13:02:08 -0400 Subject: [PATCH 2/9] Created functions to reduce memory --- iNav.lua | 259 ++++++++++++++++++++++++------------------------------- 1 file changed, 113 insertions(+), 146 deletions(-) diff --git a/iNav.lua b/iNav.lua index 41fb9021..f5cc7353 100644 --- a/iNav.lua +++ b/iNav.lua @@ -1,5 +1,4 @@ -- Lua Telemetry Flight Status Screen for INAV/Taranis-v1.1 --- -- Author: https://github.com/teckel12 -- Docs: https://github.com/iNavFlight/LuaTelemetry @@ -15,23 +14,12 @@ local X_CNTR_1=QX7 and 67 or 70 local X_CNTR_2=QX7 and 67 or 140 local X_CNTR_3=QX7 and 67 or 110 -local modeIdPrev=false -local armedPrev=false -local headingHoldPrev=false -local altHoldPrev=false -local gpsFixPrev=false -local gpsFix=false -local headingRef=-1 -local altNextPlay=0 -local battNextPlay=0 +local modeIdPrev,armedPrev,headingHoldPrev,altHoldPrev,gpsFixPrev,gpsFix,battlow,showMax=false,false,false,false,false,false,false,false +local showDir,showCurr=true,true +local headingRef,telemFlags=-1,-1 +local altNextPlay,battNextPlay=0,0 +local battPos1,battPos2=49,49 local battPercentPlayed=100 -local telemFlags=-1 -local battlow=false -local showMax=false -local showDir=true -local showCurr=true -local battPos1=49 -local battPos2=49 local modes={ {t="NO TELEM",f=FLASH,a=false,w=false}, @@ -242,14 +230,14 @@ end local function init() local rssi,low,crit=getRSSI() - local ver,radio,maj,minor,rev=getVersion() + --local ver,radio,maj,minor,rev=getVersion() local general=getGeneralSettings() data.rssiLow=low data.rssiCrit=crit - data.version=maj+minor/10 + --data.version=maj+minor/10 data.txBattMin=general["battMin"] data.txBattMax=general["battMax"] - data.units=general["imperial"] + --data.units=general["imperial"] data.modelName=model.getInfo()["name"] data.mode_id=getTelemetryId("Tmp1") data.rxBatt_id=getTelemetryId("RxBt") @@ -335,33 +323,78 @@ local function background() end end -local function drawDirection(headingDisplay,width,size,centerx,centery) - local rad1=math.rad(headingDisplay) - local rad2=math.rad(headingDisplay+width) - local rad3=math.rad(headingDisplay-width) - local x1=math.floor(math.sin(rad1)*size+0.5)+centerx - local y1=centery-math.floor(math.cos(rad1)*size+0.5) - local x2=math.floor(math.sin(rad2)*size+0.5)+centerx - local y2=centery-math.floor(math.cos(rad2)*size+0.5) - local x3=math.floor(math.sin(rad3)*size+0.5)+centerx - local y3=centery-math.floor(math.cos(rad3)*size+0.5) - lcd.drawLine(x1,y1,x2,y2,SOLID,FORCE) - lcd.drawLine(x1,y1,x3,y3,SOLID,FORCE) - if headingHold and armed then - lcd.drawFilledRectangle((x2+x3)/2-1.5,(y2+y3)/2-1.5,4,4,SOLID) - else - lcd.drawLine(x2,y2,x3,y3,DOTTED,FORCE) - end +local function gpsData(t,y,f) + lcd.drawText(85,9,t,SMLSIZE) + local x=85+(RIGHT_POS-lcd.getLastPos()) + lcd.drawText(x,y,t,SMLSIZE+f) +end + +local function drawDirection(h,w,s,x,y) + local rad1=math.rad(h) + local rad2=math.rad(h+w) + local rad3=math.rad(h-w) + local x1=math.floor(math.sin(rad1)*s+0.5)+x + local y1=y-math.floor(math.cos(rad1)*s+0.5) + local x2=math.floor(math.sin(rad2)*s+0.5)+x + local y2=y-math.floor(math.cos(rad2)*s+0.5) + local x3=math.floor(math.sin(rad3)*s+0.5)+x + local y3=y-math.floor(math.cos(rad3)*s+0.5) + lcd.drawLine(x1,y1,x2,y2,SOLID,FORCE) + lcd.drawLine(x1,y1,x3,y3,SOLID,FORCE) + if headingHold and armed then + lcd.drawFilledRectangle((x2+x3)/2-1.5,(y2+y3)/2-1.5,4,4,SOLID) + else + lcd.drawLine(x2,y2,x3,y3,DOTTED,FORCE) + end +end + +local function drawLocation() + local o1=math.rad(data.gpsHome["lat"]) + local a1=math.rad(data.gpsHome["lon"]) + local o2=math.rad(data.gpsLatLon["lat"]) + local a2=math.rad(data.gpsLatLon["lon"]) + local y=math.sin(a2-a1)*math.cos(o2) + local x=(math.cos(o1)*math.sin(o2))-(math.sin(o1)*math.cos(o2)*math.cos(a2-a1)) + local bearing=math.deg(math.atan2(y,x))-headingRef + local rad1=math.rad(bearing) + local x1=math.floor(math.sin(rad1)*10+0.5)+X_CNTR_3 + local y1=19-math.floor(math.cos(rad1)*10+0.5) + lcd.drawLine(X_CNTR_3,19,x1,y1,DOTTED,FORCE) + lcd.drawFilledRectangle(x1-1,y1-1,3,3,ERASE) + lcd.drawFilledRectangle(x1-1,y1-1,3,3,SOLID) +end + +local function drawData(t,y,d,v,m,e,p,f) + lcd.drawText(0,y,t,SMLSIZE) + if d==1 then + lcd.drawText(15,y,"\192",SMLSIZE) + elseif d==2 then + lcd.drawText(15,y,"\193",SMLSIZE) + end + if p then + lcd.drawNumber(22,y,v*10.05,SMLSIZE+PREC1+f) + else + lcd.drawText(22,y,math.floor(v+0.5),SMLSIZE+f) + end + if v0 or not gpsFix) and FLASH or 0 - value=math.floor(data.gpsAlt+0.5).."ft" - lcd.drawText(85,9,value,SMLSIZE) - pos=85+(RIGHT_POS-lcd.getLastPos()) - lcd.drawText(pos,17,value,SMLSIZE+gpsFlags) - - value=string.format("%.4f",data.gpsLatLon["lat"]) - lcd.drawText(85,9,value,SMLSIZE) - pos=85+(RIGHT_POS-lcd.getLastPos()) - lcd.drawText(pos,25,value,SMLSIZE+gpsFlags) - - value=string.format("%.4f",data.gpsLatLon["lon"]) - lcd.drawText(85,9,value,SMLSIZE) - pos=85+(RIGHT_POS-lcd.getLastPos()) - lcd.drawText(pos,33,value,SMLSIZE+gpsFlags) + local gpsFlags=(telemFlags>0 or not gpsFix) and FLASH or 0 + gpsData(math.floor(data.gpsAlt+0.5).."ft",17,gpsFlags) + gpsData(string.format("%.4f",data.gpsLatLon["lat"]),25,gpsFlags) + gpsData(string.format("%.4f",data.gpsLatLon["lon"]),33,gpsFlags) else lcd.drawFilledRectangle(RIGHT_POS-41,17,41,23,INVERS) lcd.drawText(RIGHT_POS-37,20,"No GPS",INVERS) lcd.drawText(RIGHT_POS-28,30,"Fix",INVERS) end + gpsData(" Sats "..tonumber(string.sub(data.satellites,-2)),9,telemFlags) - -- Satellites - value="Sats "..tonumber(string.sub(data.satellites,-2)) - lcd.drawText(85,9,value,SMLSIZE) - pos=85+(RIGHT_POS-lcd.getLastPos()) - lcd.drawText(85,9," ",SMLSIZE) - lcd.drawText(pos,9,value,SMLSIZE+telemFlags) - - -- Directional indicator + -- Directionals if event==EVT_ROT_LEFT or event==EVT_ROT_RIGHT or event==EVT_PLUS_BREAK or event==EVT_MINUS_BREAK then showDir=not showDir end @@ -434,34 +450,20 @@ local function run(event) end end end - if type(data.gpsLatLon)=="table" and type(data.gpsHome)=="table" and data.distLastPositive >= 25 then + if type(data.gpsLatLon)=="table" and type(data.gpsHome)=="table" and data.distLastPositive>=25 then if not showDir or not QX7 then - local o1=math.rad(data.gpsHome["lat"]) - local a1=math.rad(data.gpsHome["lon"]) - local o2=math.rad(data.gpsLatLon["lat"]) - local a2=math.rad(data.gpsLatLon["lon"]) - local y=math.sin(a2-a1)*math.cos(o2) - local x=(math.cos(o1)*math.sin(o2))-(math.sin(o1)*math.cos(o2)*math.cos(a2-a1)) - local bearing=math.deg(math.atan2(y,x))-headingRef - local rad1=math.rad(bearing) - local x1=math.floor(math.sin(rad1)*10+0.5)+X_CNTR_3 - local y1=19-math.floor(math.cos(rad1)*10+0.5) - lcd.drawLine(X_CNTR_3,19,x1,y1,DOTTED,FORCE) - lcd.drawFilledRectangle(x1-1,y1-1,3,3,ERASE) - lcd.drawFilledRectangle(x1-1,y1-1,3,3,SOLID) + drawLocation() end end - -- Head free warning - if armed and headFree then - lcd.drawText(85,9,"HF",SMLSIZE+FLASH) - end - - -- Display flight mode + -- Flight mode lcd.drawText(48,34,modes[modeId].t,SMLSIZE+modes[modeId].f) pos=MODE_POS+(87-lcd.getLastPos())/2 lcd.drawFilledRectangle(46,33,40,10,ERASE) lcd.drawText(pos,33,modes[modeId].t,SMLSIZE+modes[modeId].f) + if armed and headFree then + lcd.drawText(85,9,"HF",SMLSIZE+FLASH) + end -- Data if not armed then @@ -469,69 +471,34 @@ local function run(event) showMax=not showMax end end + local battFlags=(telemFlags>0 or battlow) and FLASH or 0 + local rssiFlags=(telemFlags>0 or data.rssi0 or battlow) and FLASH or 0 if showCurr then - lcd.drawNumber(22,33,curr*10.05,SMLSIZE+PREC1+telemFlags) - if curr<100 then - lcd.drawText(lcd.getLastPos(),33,"A",SMLSIZE+telemFlags) - end - lcd.drawText(0,41,"Fuel",SMLSIZE) - lcd.drawText(22,41,data.fuel.."%",SMLSIZE+battFlags) + drawData("Fuel",41,0,data.fuel,100,"%",false,battFlags) end - lcd.drawNumber(22,battPos1,batt*10.05,SMLSIZE+PREC1+battFlags) - lcd.drawText(lcd.getLastPos(),battPos1,"V",SMLSIZE+battFlags) - rssiFlags=(telemFlags>0 or data.rssi Date: Wed, 27 Sep 2017 17:08:20 -0400 Subject: [PATCH 3/9] Consolidated variable declaration --- iNav.lua | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/iNav.lua b/iNav.lua index f5cc7353..a1b90241 100644 --- a/iNav.lua +++ b/iNav.lua @@ -16,10 +16,8 @@ local X_CNTR_3=QX7 and 67 or 110 local modeIdPrev,armedPrev,headingHoldPrev,altHoldPrev,gpsFixPrev,gpsFix,battlow,showMax=false,false,false,false,false,false,false,false local showDir,showCurr=true,true -local headingRef,telemFlags=-1,-1 -local altNextPlay,battNextPlay=0,0 -local battPos1,battPos2=49,49 -local battPercentPlayed=100 +local headingRef,telemFlags,altNextPlay,battNextPlay=-1,-1,0,0 +local battPos1,battPos2,battPercentPlayed=49,49,100 local modes={ {t="NO TELEM",f=FLASH,a=false,w=false}, From 452c575d5255c329543e22bcb23b8b550d755f74 Mon Sep 17 00:00:00 2001 From: Tim Eckel Date: Wed, 27 Sep 2017 19:04:06 -0400 Subject: [PATCH 4/9] Reformat as it seems that doesn't change memory used --- iNav.lua | 684 +++++++++++++++++++++++++++---------------------------- 1 file changed, 342 insertions(+), 342 deletions(-) diff --git a/iNav.lua b/iNav.lua index a1b90241..159ba25a 100644 --- a/iNav.lua +++ b/iNav.lua @@ -2,41 +2,41 @@ -- Author: https://github.com/teckel12 -- Docs: https://github.com/iNavFlight/LuaTelemetry -local WAVPATH="/SCRIPTS/TELEMETRY/iNav/" -local FLASH=INVERS+BLINK -local QX7=(LCD_W<212) -local TIMER_POS=(QX7) and 60 or 150 -local RXBATT_POS=LCD_W-17 -local RIGHT_POS=(QX7) and 129 or 195 -local GAUGE_WIDTH=(QX7) and 82 or 149 -local MODE_POS=(QX7) and 48 or 90 -local X_CNTR_1=QX7 and 67 or 70 -local X_CNTR_2=QX7 and 67 or 140 -local X_CNTR_3=QX7 and 67 or 110 +local WAVPATH = "/SCRIPTS/TELEMETRY/iNav/" +local FLASH = INVERS+BLINK +local QX7 = (LCD_W<212) +local TIMER_POS = (QX7) and 60 or 150 +local RXBATT_POS = LCD_W-17 +local RIGHT_POS = (QX7) and 129 or 195 +local GAUGE_WIDTH = (QX7) and 82 or 149 +local MODE_POS = (QX7) and 48 or 90 +local X_CNTR_1 = QX7 and 67 or 70 +local X_CNTR_2 = QX7 and 67 or 140 +local X_CNTR_3 = QX7 and 67 or 110 -local modeIdPrev,armedPrev,headingHoldPrev,altHoldPrev,gpsFixPrev,gpsFix,battlow,showMax=false,false,false,false,false,false,false,false -local showDir,showCurr=true,true -local headingRef,telemFlags,altNextPlay,battNextPlay=-1,-1,0,0 -local battPos1,battPos2,battPercentPlayed=49,49,100 +local modeIdPrev, armedPrev, headingHoldPrev, altHoldPrev, gpsFixPrev, gpsFix, battlow, showMax = false, false, false, false, false, false, false, false +local showDir, showCurr = true, true +local headingRef, telemFlags, altNextPlay, battNextPlay = -1, -1, 0, 0 +local battPos1, battPos2, battPercentPlayed = 49, 49, 100 -local modes={ -{t="NO TELEM",f=FLASH,a=false,w=false}, -{t="HORIZON",f=0,a=true,w="hrznmd.wav"}, -{t="ANGLE",f=0,a=true,w="anglmd.wav"}, -{t="ACRO",f=0,a=true,w="acromd.wav"}, -{t=" NOT OK ",f=FLASH,a=false,w=false}, -{t="READY",f=0,a=false,w="ready.wav"}, -{t="POS HOLD",f=0,a=true,w="poshld.wav"}, -{t="3D HOLD",f=0,a=true,w="3dhold.wav"}, -{t="WAYPOINT",f=0,a=false,w="waypt.wav"}, -{t=" RTH ",f=FLASH,a=false,w="rtl.wav"}, -{t="FAILSAFE",f=FLASH,a=false,w="fson.wav"} +local modes = { + {t = "NO TELEM", f = FLASH, a = false, w = false}, + {t = "HORIZON", f = 0, a = true, w = "hrznmd.wav"}, + {t = "ANGLE", f = 0, a = true, w = "anglmd.wav"}, + {t = "ACRO", f = 0, a = true, w = "acromd.wav"}, + {t = " NOT OK ", f = FLASH, a = false, w = false}, + {t = "READY", f = 0, a = false, w = "ready.wav"}, + {t = "POS HOLD", f = 0, a = true, w = "poshld.wav"}, + {t = "3D HOLD", f = 0, a = true, w = "3dhold.wav"}, + {t = "WAYPOINT", f = 0, a = false, w = "waypt.wav"}, + {t = " RTH ", f = FLASH, a = false, w = "rtl.wav"}, + {t = "FAILSAFE", f = FLASH, a = false, w = "fson.wav"} } -local data={} +local data = {} local function getTelemetryId(name) - local field=getFieldInfo(name) + local field = getFieldInfo(name) if field then return field.id else @@ -45,343 +45,343 @@ local function getTelemetryId(name) end local function flightModes() - armed=false - headFree=false - headingHold=false - altHold=false - ok2arm=false - posHold=false + armed = false + headFree = false + headingHold = false + altHold = false + ok2arm = false + posHold = false if data.telemetry then - local modeA=math.floor(data.mode/10000) - local modeB=math.floor(data.mode/1000)%10 - local modeC=math.floor(data.mode/100)%10 - local modeD=math.floor(data.mode/10)%10 - local modeE=data.mode%10 - if bit32.band(modeE,4)>0 then - armed=true - if bit32.band(modeD,2)>0 then - modeId=2 - elseif bit32.band(modeD,1)>0 then - modeId=3 + local modeA = math.floor(data.mode / 10000) + local modeB = math.floor(data.mode / 1000) % 10 + local modeC = math.floor(data.mode / 100) % 10 + local modeD = math.floor(data.mode / 10) % 10 + local modeE = data.mode % 10 + if bit32.band(modeE, 4) > 0 then + armed = true + if bit32.band(modeD, 2) > 0 then + modeId = 2 + elseif bit32.band(modeD, 1) > 0 then + modeId = 3 else - modeId=4 + modeId = 4 end end - if bit32.band(modeE,2)>0 or modeE==0 then - modeId=5 + if bit32.band(modeE, 2) > 0 or modeE == 0 then + modeId = 5 else - ok2arm=true + ok2arm = true if not armed then - modeId=6 + modeId = 6 end end - if bit32.band(modeB,4)>0 then - headFree=true + if bit32.band(modeB, 4) > 0 then + headFree = true end - if bit32.band(modeC,4)>0 then + if bit32.band(modeC, 4) > 0 then if armed then - modeId=7 - posHold=true + modeId = 7 + posHold = true end end - if bit32.band(modeC,2)>0 then - altHold=true + if bit32.band(modeC, 2) > 0 then + altHold = true if posHold then - modeId=8 + modeId = 8 end end - if bit32.band(modeC,1)>0 then - headingHold=true + if bit32.band(modeC, 1) > 0 then + headingHold = true end - if bit32.band(modeB,2)>0 then - modeId=9 + if bit32.band(modeB, 2) > 0 then + modeId = 9 end - if bit32.band(modeB,4)>0 then - modeId=10 + if bit32.band(modeB, 4) > 0 then + modeId = 10 end - if bit32.band(modeA,4)>0 then - modeId=11 + if bit32.band(modeA, 4) > 0 then + modeId = 11 end else - modeId=1 + modeId = 1 end -- Audio flight mode feedback - local vibrate=false - local beep=false + local vibrate = false + local beep = false if armed and not armedPrev then - data.timerStart=getTime() - data.distLastPositive=0 - headingRef=data.heading - data.gpsHome=false - battPercentPlayed=100 - battlow=false - showMax=false - showDir=false - playFile(WAVPATH.."engarm.wav") + data.timerStart = getTime() + data.distLastPositive = 0 + headingRef = data.heading + data.gpsHome = false + battPercentPlayed = 100 + battlow = false + showMax = false + showDir = false + playFile(WAVPATH .. "engarm.wav") elseif not armed and armedPrev then - if data.distLastPositive<15 then - headingRef=-1 - showDir=true + if data.distLastPositive < 15 then + headingRef = -1 + showDir = true end - playFile(WAVPATH.."engdrm.wav") + playFile(WAVPATH .. "engdrm.wav") end if gpsFix and not gpsFixPrev then - playFile(WAVPATH.."gps.wav") - playFile(WAVPATH.."good.wav") + playFile(WAVPATH .. "gps.wav") + playFile(WAVPATH .. "good.wav") elseif not gpsFix and gpsFixPrev then - playFile(WAVPATH.."gps.wav") - playFile(WAVPATH.."lost.wav") + playFile(WAVPATH .. "gps.wav") + playFile(WAVPATH .. "lost.wav") end - if modeIdPrev and modeIdPrev~=modeId then - if not armed and modeId==6 and modeIdPrev==5 then - playFile(WAVPATH..modes[modeId].w) + if modeIdPrev and modeIdPrev ~= modeId then + if not armed and modeId == 6 and modeIdPrev == 5 then + playFile(WAVPATH .. modes[modeId].w) end if armed then if modes[modeId].w then - playFile(WAVPATH..modes[modeId].w) + playFile(WAVPATH .. modes[modeId].w) end - if modes[modeId].f>0 then - vibrate=true + if modes[modeId].f > 0 then + vibrate = true end end end if armed then - if altHold and modes[modeId].a and altHoldPrev~=altHold then - playFile(WAVPATH.."althld.wav") - playFile(WAVPATH.."active.wav") - elseif not altHold and modes[modeId].a and altHoldPrev~=altHold then - playFile(WAVPATH.."althld.wav") - playFile(WAVPATH.."off.wav") + if altHold and modes[modeId].a and altHoldPrev ~= altHold then + playFile(WAVPATH .. "althld.wav") + playFile(WAVPATH .. "active.wav") + elseif not altHold and modes[modeId].a and altHoldPrev ~= altHold then + playFile(WAVPATH .. "althld.wav") + playFile(WAVPATH .. "off.wav") end - if headingHold and headingHoldPrev~=headingHold then - playFile(WAVPATH.."hedhlda.wav") - elseif not headingHold and headingHoldPrev~=headingHold then - playFile(WAVPATH.."hedhld.wav") - playFile(WAVPATH.."off.wav") + if headingHold and headingHoldPrev ~= headingHold then + playFile(WAVPATH .. "hedhlda.wav") + elseif not headingHold and headingHoldPrev ~= headingHold then + playFile(WAVPATH .. "hedhld.wav") + playFile(WAVPATH .. "off.wav") end - if data.altitude>400 then - if getTime()>altNextPlay then - playNumber(data.altitude,10) - altNextPlay=getTime()+1000 + if data.altitude > 400 then + if getTime() > altNextPlay then + playNumber(data.altitude, 10) + altNextPlay = getTime() + 1000 else - beep=true + beep = true end end - if battPercentPlayed>data.fuel then - if data.fuel==30 or data.fuel==25 then - playFile(WAVPATH.."batlow.wav") - playNumber(data.fuel,13) - battPercentPlayed=data.fuel - elseif data.fuel%10==0 and data.fuel<100 and data.fuel>=40 then - playFile(WAVPATH.."battry.wav") - playNumber(data.fuel,13) - battPercentPlayed=data.fuel + if battPercentPlayed > data.fuel then + if data.fuel == 30 or data.fuel == 25 then + playFile(WAVPATH .. "batlow.wav") + playNumber(data.fuel, 13) + battPercentPlayed = data.fuel + elseif data.fuel%10 == 0 and data.fuel < 100 and data.fuel >= 40 then + playFile(WAVPATH .. "battry.wav") + playNumber(data.fuel, 13) + battPercentPlayed = data.fuel end end - if data.fuel<=20 or data.cell<3.40 then - if getTime()>battNextPlay then - playFile(WAVPATH.."batcrt.wav") - if data.fuel<=20 and battPercentPlayed>data.fuel then - playNumber(data.fuel,13) - battPercentPlayed=data.fuel + if data.fuel <= 20 or data.cell < 3.40 then + if getTime() > battNextPlay then + playFile(WAVPATH .. "batcrt.wav") + if data.fuel <= 20 and battPercentPlayed > data.fuel then + playNumber(data.fuel, 13) + battPercentPlayed = data.fuel end - battNextPlay=getTime()+500 + battNextPlay = getTime()+500 else - vibrate=true - beep=true + vibrate = true + beep = true end - battlow=true - elseif data.cell<3.50 then + battlow = true + elseif data.cell < 3.50 then if not battlow then - playFile(WAVPATH.."batlow.wav") - battlow=true + playFile(WAVPATH .. "batlow.wav") + battlow = true end else - battNextPlay=0 + battNextPlay = 0 end - if headFree or modes[modeId].f>0 then - beep=true + if headFree or modes[modeId].f > 0 then + beep = true end - if data.rssi0 or telemFlags<0 then - data.telemetry=true - data.mode=getValue(data.mode_id) - data.rxBatt=getValue(data.rxBatt_id) - data.satellites=getValue(data.satellites_id) - data.gpsAlt=getValue(data.gpsAlt_id) - data.gpsLatLon=getValue(data.gpsLatLon_id) - data.heading=getValue(data.heading_id) - data.altitude=getValue(data.altitude_id) - data.distance=math.floor(getValue(data.distance_id)*3.28084+0.5) - data.speed=getValue(data.speed_id) + data.rssi = getValue(data.rssi_id) + if data.rssi > 0 or telemFlags < 0 then + data.telemetry = true + data.mode = getValue(data.mode_id) + data.rxBatt = getValue(data.rxBatt_id) + data.satellites = getValue(data.satellites_id) + data.gpsAlt = getValue(data.gpsAlt_id) + data.gpsLatLon = getValue(data.gpsLatLon_id) + data.heading = getValue(data.heading_id) + data.altitude = getValue(data.altitude_id) + data.distance = math.floor(getValue(data.distance_id) * 3.28084 + 0.5) + data.speed = getValue(data.speed_id) if showCurr then - data.current=getValue(data.current_id) - data.currentMax=getValue(data.currentMax_id) - data.fuel=getValue(data.fuel_id) + data.current = getValue(data.current_id) + data.currentMax = getValue(data.currentMax_id) + data.fuel = getValue(data.fuel_id) end - data.altitudeMax=getValue(data.altitudeMax_id) - data.distanceMax=getValue(data.distanceMax_id) - data.speedMax=getValue(data.speedMax_id) - data.batt=getValue(data.batt_id) - data.battMin=getValue(data.battMin_id) - data.cells=math.floor(data.batt/4.3)+1 - data.cell=data.batt/data.cells - data.cellMin=data.battMin/data.cells - data.rssiMin=getValue(data.rssiMin_id) - data.txBatt=getValue(data.txBatt_id) - data.rssiLast=data.rssi - --data.distance=237 - --data.gpsLatLon["lat"]=math.deg(data.gpsLatLon["lat"]) - --data.gpsLatLon["lon"]=math.deg(data.gpsLatLon["lon"]*2.2) - if data.distance>0 then - data.distLastPositive=data.distance + data.altitudeMax = getValue(data.altitudeMax_id) + data.distanceMax = getValue(data.distanceMax_id) + data.speedMax = getValue(data.speedMax_id) + data.batt = getValue(data.batt_id) + data.battMin = getValue(data.battMin_id) + data.cells = math.floor(data.batt/4.3)+1 + data.cell = data.batt/data.cells + data.cellMin = data.battMin/data.cells + data.rssiMin = getValue(data.rssiMin_id) + data.txBatt = getValue(data.txBatt_id) + data.rssiLast = data.rssi + --data.distance = 237 + --data.gpsLatLon["lat"] = math.deg(data.gpsLatLon["lat"]) + --data.gpsLatLon["lon"] = math.deg(data.gpsLatLon["lon"]*2.2) + if data.distance > 0 then + data.distLastPositive = data.distance end - gpsFix=(type(data.gpsLatLon)=="table" and data.satellites>3900) - telemFlags=0 + gpsFix = (type(data.gpsLatLon) == "table" and data.satellites > 3900) + telemFlags = 0 else - data.telemetry=false - telemFlags=FLASH + data.telemetry = false + telemFlags = FLASH end flightModes() - if armed and gpsFix and type(data.gpsLatLon)=="table" and type(data.gpsHome)~="table" then - data.gpsHome=data.gpsLatLon + if armed and gpsFix and type(data.gpsLatLon) == "table" and type(data.gpsHome) ~= "table" then + data.gpsHome = data.gpsLatLon end end -local function gpsData(t,y,f) - lcd.drawText(85,9,t,SMLSIZE) - local x=85+(RIGHT_POS-lcd.getLastPos()) - lcd.drawText(x,y,t,SMLSIZE+f) +local function gpsData(t, y, f) + lcd.drawText(85, 9, t, SMLSIZE) + local x = 85 + (RIGHT_POS-lcd.getLastPos()) + lcd.drawText(x, y, t, SMLSIZE + f) end -local function drawDirection(h,w,s,x,y) - local rad1=math.rad(h) - local rad2=math.rad(h+w) - local rad3=math.rad(h-w) - local x1=math.floor(math.sin(rad1)*s+0.5)+x - local y1=y-math.floor(math.cos(rad1)*s+0.5) - local x2=math.floor(math.sin(rad2)*s+0.5)+x - local y2=y-math.floor(math.cos(rad2)*s+0.5) - local x3=math.floor(math.sin(rad3)*s+0.5)+x - local y3=y-math.floor(math.cos(rad3)*s+0.5) - lcd.drawLine(x1,y1,x2,y2,SOLID,FORCE) - lcd.drawLine(x1,y1,x3,y3,SOLID,FORCE) +local function drawDirection(h, w, s, x, y) + local rad1 = math.rad(h) + local rad2 = math.rad(h + w) + local rad3 = math.rad(h - w) + local x1 = math.floor(math.sin(rad1) * s + 0.5) + x + local y1 = y - math.floor(math.cos(rad1) * s + 0.5) + local x2 = math.floor(math.sin(rad2) * s + 0.5) + x + local y2 = y - math.floor(math.cos(rad2) * s + 0.5) + local x3 = math.floor(math.sin(rad3) * s + 0.5) + x + local y3 = y - math.floor(math.cos(rad3) * s + 0.5) + lcd.drawLine(x1, y1, x2, y2, SOLID, FORCE) + lcd.drawLine(x1, y1, x3, y3, SOLID, FORCE) if headingHold and armed then - lcd.drawFilledRectangle((x2+x3)/2-1.5,(y2+y3)/2-1.5,4,4,SOLID) + lcd.drawFilledRectangle((x2 + x3) / 2 - 1.5, (y2 + y3) / 2 - 1.5, 4, 4, SOLID) else - lcd.drawLine(x2,y2,x3,y3,DOTTED,FORCE) + lcd.drawLine(x2, y2, x3, y3, DOTTED, FORCE) end end local function drawLocation() - local o1=math.rad(data.gpsHome["lat"]) - local a1=math.rad(data.gpsHome["lon"]) - local o2=math.rad(data.gpsLatLon["lat"]) - local a2=math.rad(data.gpsLatLon["lon"]) - local y=math.sin(a2-a1)*math.cos(o2) - local x=(math.cos(o1)*math.sin(o2))-(math.sin(o1)*math.cos(o2)*math.cos(a2-a1)) - local bearing=math.deg(math.atan2(y,x))-headingRef - local rad1=math.rad(bearing) - local x1=math.floor(math.sin(rad1)*10+0.5)+X_CNTR_3 - local y1=19-math.floor(math.cos(rad1)*10+0.5) - lcd.drawLine(X_CNTR_3,19,x1,y1,DOTTED,FORCE) - lcd.drawFilledRectangle(x1-1,y1-1,3,3,ERASE) - lcd.drawFilledRectangle(x1-1,y1-1,3,3,SOLID) + local o1 = math.rad(data.gpsHome["lat"]) + local a1 = math.rad(data.gpsHome["lon"]) + local o2 = math.rad(data.gpsLatLon["lat"]) + local a2 = math.rad(data.gpsLatLon["lon"]) + local y = math.sin(a2 - a1)*math.cos(o2) + local x = (math.cos(o1) * math.sin(o2)) - (math.sin(o1) * math.cos(o2) * math.cos(a2 - a1)) + local bearing = math.deg(math.atan2(y, x)) - headingRef + local rad1 = math.rad(bearing) + local x1 = math.floor(math.sin(rad1) * 10 + 0.5) + X_CNTR_3 + local y1 = 19 - math.floor(math.cos(rad1) * 10 + 0.5) + lcd.drawLine(X_CNTR_3, 19, x1, y1, DOTTED, FORCE) + lcd.drawFilledRectangle(x1 - 1, y1 - 1, 3, 3, ERASE) + lcd.drawFilledRectangle(x1 - 1, y1 - 1, 3, 3, SOLID) end -local function drawData(t,y,d,v,m,e,p,f) - lcd.drawText(0,y,t,SMLSIZE) - if d==1 then - lcd.drawText(15,y,"\192",SMLSIZE) - elseif d==2 then - lcd.drawText(15,y,"\193",SMLSIZE) +local function drawData(t, y, d, v, m, e, p, f) + lcd.drawText(0, y, t, SMLSIZE) + if d == 1 then + lcd.drawText(15, y, "\192", SMLSIZE) + elseif d == 2 then + lcd.drawText(15, y, "\193", SMLSIZE) end if p then - lcd.drawNumber(22,y,v*10.05,SMLSIZE+PREC1+f) + lcd.drawNumber(22, y, v * 10.05, SMLSIZE + PREC1 + f) else - lcd.drawText(22,y,math.floor(v+0.5),SMLSIZE+f) + lcd.drawText(22, y, math.floor(v + 0.5), SMLSIZE + f) end - if v0 and data.telemetry then - lcd.drawNumber(RXBATT_POS,1,data.rxBatt*10.05,SMLSIZE+PREC1+INVERS) - lcd.drawText(lcd.getLastPos(),1,"V",SMLSIZE+INVERS) + lcd.drawNumber(RXBATT_POS, 1, data.rxBatt * 10.05, SMLSIZE + PREC1 + INVERS) + lcd.drawText(lcd.getLastPos(), 1, "V", SMLSIZE + INVERS) end -- GPS - if type(data.gpsLatLon)=="table" then - local gpsFlags=(telemFlags>0 or not gpsFix) and FLASH or 0 - gpsData(math.floor(data.gpsAlt+0.5).."ft",17,gpsFlags) - gpsData(string.format("%.4f",data.gpsLatLon["lat"]),25,gpsFlags) - gpsData(string.format("%.4f",data.gpsLatLon["lon"]),33,gpsFlags) + if type(data.gpsLatLon) == "table" then + local gpsFlags = (telemFlags > 0 or not gpsFix) and FLASH or 0 + gpsData(math.floor(data.gpsAlt + 0.5) .. "ft", 17, gpsFlags) + gpsData(string.format("%.4f", data.gpsLatLon["lat"]), 25, gpsFlags) + gpsData(string.format("%.4f", data.gpsLatLon["lon"]), 33, gpsFlags) else - lcd.drawFilledRectangle(RIGHT_POS-41,17,41,23,INVERS) - lcd.drawText(RIGHT_POS-37,20,"No GPS",INVERS) - lcd.drawText(RIGHT_POS-28,30,"Fix",INVERS) + lcd.drawFilledRectangle(RIGHT_POS - 41, 17, 41, 23, INVERS) + lcd.drawText(RIGHT_POS - 37, 20, "No GPS", INVERS) + lcd.drawText(RIGHT_POS - 28, 30, "Fix", INVERS) end - gpsData(" Sats "..tonumber(string.sub(data.satellites,-2)),9,telemFlags) + gpsData(" Sats " .. tonumber(string.sub(data.satellites, -2)), 9, telemFlags) -- Directionals - if event==EVT_ROT_LEFT or event==EVT_ROT_RIGHT or event==EVT_PLUS_BREAK or event==EVT_MINUS_BREAK then - showDir=not showDir + if event == EVT_ROT_LEFT or event == EVT_ROT_RIGHT or event == EVT_PLUS_BREAK or event == EVT_MINUS_BREAK then + showDir = not showDir end if data.telemetry then - local indicatorDisplayed=false + local indicatorDisplayed = false if showDir or headingRef<0 or not QX7 then - lcd.drawText(X_CNTR_1-2,9,"N "..math.floor(data.heading+0.5).."\64",SMLSIZE) - lcd.drawText(X_CNTR_1+10,21,"E",SMLSIZE) - lcd.drawText(X_CNTR_1-14,21,"W",SMLSIZE) - drawDirection(data.heading,135,7,X_CNTR_1,23) - indicatorDisplayed=true + lcd.drawText(X_CNTR_1 - 2, 9, "N " .. math.floor(data.heading + 0.5) .. "\64", SMLSIZE) + lcd.drawText(X_CNTR_1 + 10, 21, "E", SMLSIZE) + lcd.drawText(X_CNTR_1 - 14, 21, "W", SMLSIZE) + drawDirection(data.heading, 135, 7, X_CNTR_1, 23) + indicatorDisplayed = true end - if not showDir or headingRef>=0 or not QX7 then + if not showDir or headingRef >= 0 or not QX7 then if not indicatorDisplayed or not QX7 then - drawDirection(data.heading-headingRef,145,10,X_CNTR_2,19) + drawDirection(data.heading-headingRef, 145, 10, X_CNTR_2, 19) end end end - if type(data.gpsLatLon)=="table" and type(data.gpsHome)=="table" and data.distLastPositive>=25 then + if type(data.gpsLatLon) == "table" and type(data.gpsHome) == "table" and data.distLastPositive >= 25 then if not showDir or not QX7 then drawLocation() end end -- Flight mode - lcd.drawText(48,34,modes[modeId].t,SMLSIZE+modes[modeId].f) - pos=MODE_POS+(87-lcd.getLastPos())/2 - lcd.drawFilledRectangle(46,33,40,10,ERASE) - lcd.drawText(pos,33,modes[modeId].t,SMLSIZE+modes[modeId].f) + lcd.drawText(48, 34, modes[modeId].t, SMLSIZE + modes[modeId].f) + pos = MODE_POS + (87 - lcd.getLastPos()) / 2 + lcd.drawFilledRectangle(46, 33, 40, 10, ERASE) + lcd.drawText(pos, 33, modes[modeId].t, SMLSIZE + modes[modeId].f) if armed and headFree then - lcd.drawText(85,9,"HF",SMLSIZE+FLASH) + lcd.drawText(85, 9, "HF", SMLSIZE + FLASH) end -- Data if not armed then - if event==EVT_ROT_LEFT or event==EVT_ROT_RIGHT or event==EVT_PLUS_BREAK or event==EVT_MINUS_BREAK then - showMax=not showMax + if event == EVT_ROT_LEFT or event == EVT_ROT_RIGHT or event == EVT_PLUS_BREAK or event == EVT_MINUS_BREAK then + showMax = not showMax end end - local battFlags=(telemFlags>0 or battlow) and FLASH or 0 - local rssiFlags=(telemFlags>0 or data.rssi 0 or battlow) and FLASH or 0 + local rssiFlags = (telemFlags > 0 or data.rssi < data.rssiLow) and FLASH or 0 if showMax then - drawData("Alt",9,1,data.altitudeMax,1000,"ft",false,telemFlags) + drawData("Alt", 9, 1, data.altitudeMax, 1000, "ft", false, telemFlags) drawAltHold() - drawData("Dst",17,1,data.distanceMax*3.28084,1000,"ft",false,telemFlags) - drawData("Spd",25,1,data.speedMax,100,"mph",false,telemFlags) + drawData("Dst", 17, 1, data.distanceMax * 3.28084, 1000, "ft", false, telemFlags) + drawData("Spd", 25, 1, data.speedMax, 100, "mph", false, telemFlags) if showCurr then - drawData("Cur",33,1,data.currentMax,100,"A",true,telemFlags) + drawData("Cur", 33, 1, data.currentMax, 100, "A", true, telemFlags) end - drawData("Bat",battPos1,2,data.battMin,100,"V",true,battFlags) - drawData("RSI",57,2,data.rssiMin,100,"dB",false,rssiFlags) + drawData("Bat", battPos1, 2, data.battMin, 100, "V", true, battFlags) + drawData("RSI", 57, 2, data.rssiMin, 100, "dB", false, rssiFlags) else - drawData("Altd",9,0,data.altitude,1000,"ft",false,telemFlags) + drawData("Altd", 9, 0, data.altitude, 1000, "ft", false, telemFlags) drawAltHold() - drawData("Dist",17,0,data.distLastPositive*3.28084,1000,"ft",false,telemFlags) - drawData("Sped",25,0,data.speed,100,"mph",false,telemFlags) + drawData("Dist", 17, 0, data.distLastPositive, 1000, "ft", false, telemFlags) + drawData("Sped", 25, 0, data.speed, 100, "mph", false, telemFlags) if showCurr then - drawData("Curr",33,0,data.current,100,"A",true,telemFlags) + drawData("Curr", 33, 0, data.current, 100, "A", true, telemFlags) end - drawData("Batt",battPos1,0,data.batt,100,"V",true,battFlags) - drawData("RSSI",57,0,data.rssiLast,100,"dB",false,rssiFlags) + drawData("Batt", battPos1, 0, data.batt, 100, "V", true, battFlags) + drawData("RSSI", 57, 0, data.rssiLast, 100, "dB", false, rssiFlags) end if showCurr then - drawData("Fuel",41,0,data.fuel,100,"%",false,battFlags) + drawData("Fuel", 41, 0, data.fuel, 100, "%", false, battFlags) end -- Graphs if showCurr then - lcd.drawGauge(46,41,GAUGE_WIDTH,7,math.min(data.fuel,98),100) - if data.fuel==0 then - lcd.drawLine(47,42,47,46,SOLID,ERASE) + lcd.drawGauge(46, 41, GAUGE_WIDTH, 7, math.min(data.fuel, 98), 100) + if data.fuel == 0 then + lcd.drawLine(47, 42, 47, 46, SOLID, ERASE) end end - lcd.drawGauge(46,battPos2,GAUGE_WIDTH,56-battPos2,math.min(math.max(data.cell-3.3,0)*111.1,98),100) - min=(GAUGE_WIDTH-2)*(math.min(math.max(data.cellMin-3.3,0)*111.1,99)/100)+47 - lcd.drawLine(min,battPos2+1,min,54,SOLID,ERASE) - local rssiGauge=math.max(math.min((data.rssiLast-data.rssiCrit)/(100-data.rssiCrit)*100,98),0) - lcd.drawGauge(46,57,GAUGE_WIDTH,7,rssiGauge,100) - min=(GAUGE_WIDTH-2)*(math.max(math.min((data.rssiMin-data.rssiCrit)/(100-data.rssiCrit)*100,99),0)/100)+47 - lcd.drawLine(min,58,min,62,SOLID,ERASE) + lcd.drawGauge(46, battPos2, GAUGE_WIDTH, 56 - battPos2, math.min(math.max(data.cell - 3.3, 0) * 111.1, 98), 100) + min = (GAUGE_WIDTH - 2) * (math.min(math.max(data.cellMin - 3.3, 0) * 111.1, 99) / 100) + 47 + lcd.drawLine(min, battPos2 + 1, min, 54, SOLID, ERASE) + local rssiGauge = math.max(math.min((data.rssiLast - data.rssiCrit) / (100 - data.rssiCrit) * 100, 98), 0) + lcd.drawGauge(46, 57, GAUGE_WIDTH, 7, rssiGauge, 100) + min = (GAUGE_WIDTH - 2) * (math.max(math.min((data.rssiMin - data.rssiCrit) / (100 - data.rssiCrit) * 100, 99), 0) / 100) + 47 + lcd.drawLine(min, 58, min, 62, SOLID, ERASE) --if not QX7 then - -- lcd.drawRectangle(199,9,13,48,SOLID) - -- local height=math.max(math.min(math.ceil(data.altitude/400*46),46),0) - -- lcd.drawFilledRectangle(200,56-height,11,height,INVERS) - -- local max=56-math.max(math.min(math.ceil(data.altitudeMax/400*46),46),0) - -- lcd.drawLine(200,max,210,max,DOTTED,FORCE) - -- lcd.drawText(199,58,"Alt",SMLSIZE) + -- lcd.drawRectangle(199, 9, 13, 48, SOLID) + -- local height = math.max(math.min(math.ceil(data.altitude / 400 * 46), 46), 0) + -- lcd.drawFilledRectangle(200, 56 - height, 11, height, INVERS) + -- local max = 56 - math.max(math.min(math.ceil(data.altitudeMax / 400 * 46), 46), 0) + -- lcd.drawLine(200, max, 210, max, DOTTED, FORCE) + -- lcd.drawText(199, 58, "Alt", SMLSIZE) --end return 1 end -return {init=init,run=run,background=background} \ No newline at end of file +return {init = init, run = run, background = background} \ No newline at end of file From fcef60ae57699e0b0a37b6236c58fe674c080f82 Mon Sep 17 00:00:00 2001 From: Tim Eckel Date: Thu, 28 Sep 2017 10:02:49 -0400 Subject: [PATCH 5/9] Update README.md --- README.md | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 7b2af0e7..48728d3c 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # Lua Telemetry Flight Status Screen for INAV/Taranis -> Note: Currently only tested on Q X7, feedback on X9D, X9D+ & X9E appreciated. +> **Notice** If you get the message `Script Panic not enough memory` you've run out of memory on your transmitter. +> This happens if you have too many Lua scripts running (this includes model, function, and telemetry scripts). +> It's also possible that you have less memory to work with when running firmware that uses more memory (for example, firmware that includes multimodule support). #### Taranis Q X7 ![sample](http://www.leethost.com/link_pics/iNav1.png "Launch-based model orientation and location indicators") @@ -30,10 +32,10 @@ #### Notes -* Designed to work on a Taranis Q X7, X9D, X9D+ & X9E **(currently only tested on Q X7)** -* Designed for a multirotor model, but should be valuable for fixed wing (fixed wing feedback would be appreciated) +* Designed for the Taranis Q X7, X9D, X9D+ & X9E +* Designed for multirotor models, but should be valuable for fixed wing (fixed wing feedback would be appreciated) * Optional amperage sensor needed for fuel and current displays -* Uses Taranis settings for RSSI warning/critical levels for graph and audio/vibration warnings +* Uses Taranis settings for RSSI warning/critical levels for graph range and audio/haptic warnings * Uses Taranis settings for transmitter voltage min/max for battery graphic in screen title ## Setup @@ -72,10 +74,10 @@ Audio feedback will play in background even if iNav LuaTelemetry screen is not d ## Release History -#### v1.1 - 09/22/2017 +#### v1.1.0 - 09/22/2017 * Repo moved to INAVFlight * Screen formatting for Taranis X9D, X9D+ & X9E -#### v1.0 - 09/19/2017 +#### v1.0.0 - 09/19/2017 * Initial release ## Todo From a9b8d61e5847a41dd198b7f807d37eab1467c183 Mon Sep 17 00:00:00 2001 From: teckel12 Date: Thu, 28 Sep 2017 10:36:25 -0400 Subject: [PATCH 6/9] Update readme notice to include comment about enabling luac in firmware --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 48728d3c..cb738d84 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,7 @@ > **Notice** If you get the message `Script Panic not enough memory` you've run out of memory on your transmitter. > This happens if you have too many Lua scripts running (this includes model, function, and telemetry scripts). > It's also possible that you have less memory to work with when running firmware that uses more memory (for example, firmware that includes multimodule support). +> Using firmware with `luac` included should reduce memory usage and increase the telemetry screen speed. #### Taranis Q X7 ![sample](http://www.leethost.com/link_pics/iNav1.png "Launch-based model orientation and location indicators") From d822984cd8eb905a97fd52f9538caca150080c59 Mon Sep 17 00:00:00 2001 From: teckel12 Date: Thu, 28 Sep 2017 10:48:56 -0400 Subject: [PATCH 7/9] Code formatting changes --- iNav.lua | 90 ++++++++++++++++++++++++++++++++------------------------ 1 file changed, 52 insertions(+), 38 deletions(-) diff --git a/iNav.lua b/iNav.lua index 159ba25a..e46784c7 100644 --- a/iNav.lua +++ b/iNav.lua @@ -3,7 +3,7 @@ -- Docs: https://github.com/iNavFlight/LuaTelemetry local WAVPATH = "/SCRIPTS/TELEMETRY/iNav/" -local FLASH = INVERS+BLINK +local FLASH = INVERS + BLINK local QX7 = (LCD_W<212) local TIMER_POS = (QX7) and 60 or 150 local RXBATT_POS = LCD_W-17 @@ -14,23 +14,41 @@ local X_CNTR_1 = QX7 and 67 or 70 local X_CNTR_2 = QX7 and 67 or 140 local X_CNTR_3 = QX7 and 67 or 110 -local modeIdPrev, armedPrev, headingHoldPrev, altHoldPrev, gpsFixPrev, gpsFix, battlow, showMax = false, false, false, false, false, false, false, false -local showDir, showCurr = true, true -local headingRef, telemFlags, altNextPlay, battNextPlay = -1, -1, 0, 0 -local battPos1, battPos2, battPercentPlayed = 49, 49, 100 +local modeIdPrev = false +local armedPrev = false +local headingHoldPrev = false +local altHoldPrev = false +local gpsFixPrev = false +local gpsFix = false +local headingRef = -1 +local altNextPlay = 0 +local battNextPlay = 0 +local battPercentPlayed = 100 +local telemFlags = -1 +local battlow = false +local showMax = false +local showDir = true +local showCurr = true +local battPos1 = 49 +local battPos2 = 49 +-- Modes +-- t = text +-- f = flags for text +-- a = show alititude hold +-- w = wave file local modes = { - {t = "NO TELEM", f = FLASH, a = false, w = false}, - {t = "HORIZON", f = 0, a = true, w = "hrznmd.wav"}, - {t = "ANGLE", f = 0, a = true, w = "anglmd.wav"}, - {t = "ACRO", f = 0, a = true, w = "acromd.wav"}, - {t = " NOT OK ", f = FLASH, a = false, w = false}, - {t = "READY", f = 0, a = false, w = "ready.wav"}, - {t = "POS HOLD", f = 0, a = true, w = "poshld.wav"}, - {t = "3D HOLD", f = 0, a = true, w = "3dhold.wav"}, - {t = "WAYPOINT", f = 0, a = false, w = "waypt.wav"}, - {t = " RTH ", f = FLASH, a = false, w = "rtl.wav"}, - {t = "FAILSAFE", f = FLASH, a = false, w = "fson.wav"} + { t="NO TELEM", f=FLASH, a=false, w=false }, + { t="HORIZON", f=0, a=true, w="hrznmd.wav" }, + { t="ANGLE", f=0, a=true, w="anglmd.wav" }, + { t="ACRO", f=0, a=true, w="acromd.wav" }, + { t=" NOT OK ", f=FLASH, a=false, w=false }, + { t="READY", f=0, a=false, w="ready.wav" }, + { t="POS HOLD", f=0, a=true, w="poshld.wav" }, + { t="3D HOLD", f=0, a=true, w="3dhold.wav" }, + { t="WAYPOINT", f=0, a=false, w="waypt.wav" }, + { t=" RTH ", f=FLASH, a=false, w="rtl.wav" }, + { t="FAILSAFE", f=FLASH, a=false, w="fson.wav" }, } local data = {} @@ -106,7 +124,7 @@ local function flightModes() modeId = 1 end - -- Audio flight mode feedback + -- Flight mode feedback local vibrate = false local beep = false if armed and not armedPrev then @@ -173,7 +191,7 @@ local function flightModes() playFile(WAVPATH .. "batlow.wav") playNumber(data.fuel, 13) battPercentPlayed = data.fuel - elseif data.fuel%10 == 0 and data.fuel < 100 and data.fuel >= 40 then + elseif data.fuel % 10 == 0 and data.fuel < 100 and data.fuel >= 40 then playFile(WAVPATH .. "battry.wav") playNumber(data.fuel, 13) battPercentPlayed = data.fuel @@ -232,7 +250,7 @@ local function init() local general = getGeneralSettings() data.rssiLow = low data.rssiCrit = crit - --data.version = maj+minor/10 + --data.version = maj + minor / 10 data.txBattMin = general["battMin"] data.txBattMax = general["battMax"] --data.units = general["imperial"] @@ -346,22 +364,6 @@ local function drawDirection(h, w, s, x, y) end end -local function drawLocation() - local o1 = math.rad(data.gpsHome["lat"]) - local a1 = math.rad(data.gpsHome["lon"]) - local o2 = math.rad(data.gpsLatLon["lat"]) - local a2 = math.rad(data.gpsLatLon["lon"]) - local y = math.sin(a2 - a1)*math.cos(o2) - local x = (math.cos(o1) * math.sin(o2)) - (math.sin(o1) * math.cos(o2) * math.cos(a2 - a1)) - local bearing = math.deg(math.atan2(y, x)) - headingRef - local rad1 = math.rad(bearing) - local x1 = math.floor(math.sin(rad1) * 10 + 0.5) + X_CNTR_3 - local y1 = 19 - math.floor(math.cos(rad1) * 10 + 0.5) - lcd.drawLine(X_CNTR_3, 19, x1, y1, DOTTED, FORCE) - lcd.drawFilledRectangle(x1 - 1, y1 - 1, 3, 3, ERASE) - lcd.drawFilledRectangle(x1 - 1, y1 - 1, 3, 3, SOLID) -end - local function drawData(t, y, d, v, m, e, p, f) lcd.drawText(0, y, t, SMLSIZE) if d == 1 then @@ -411,7 +413,7 @@ local function run(event) lcd.drawNumber(110 , 1, data.txBatt * 10.05, SMLSIZE + PREC1 + INVERS) lcd.drawText(lcd.getLastPos(), 1, "V", SMLSIZE + INVERS) end - if data.rxBatt>0 and data.telemetry then + if data.rxBatt > 0 and data.telemetry then lcd.drawNumber(RXBATT_POS, 1, data.rxBatt * 10.05, SMLSIZE + PREC1 + INVERS) lcd.drawText(lcd.getLastPos(), 1, "V", SMLSIZE + INVERS) end @@ -435,7 +437,7 @@ local function run(event) end if data.telemetry then local indicatorDisplayed = false - if showDir or headingRef<0 or not QX7 then + if showDir or headingRef < 0 or not QX7 then lcd.drawText(X_CNTR_1 - 2, 9, "N " .. math.floor(data.heading + 0.5) .. "\64", SMLSIZE) lcd.drawText(X_CNTR_1 + 10, 21, "E", SMLSIZE) lcd.drawText(X_CNTR_1 - 14, 21, "W", SMLSIZE) @@ -450,7 +452,19 @@ local function run(event) end if type(data.gpsLatLon) == "table" and type(data.gpsHome) == "table" and data.distLastPositive >= 25 then if not showDir or not QX7 then - drawLocation() + local o1 = math.rad(data.gpsHome["lat"]) + local a1 = math.rad(data.gpsHome["lon"]) + local o2 = math.rad(data.gpsLatLon["lat"]) + local a2 = math.rad(data.gpsLatLon["lon"]) + local y = math.sin(a2 - a1) * math.cos(o2) + local x = (math.cos(o1) * math.sin(o2)) - (math.sin(o1) * math.cos(o2) * math.cos(a2 - a1)) + local bearing = math.deg(math.atan2(y, x)) - headingRef + local rad1 = math.rad(bearing) + local x1 = math.floor(math.sin(rad1) * 10 + 0.5) + X_CNTR_3 + local y1 = 19 - math.floor(math.cos(rad1) * 10 + 0.5) + lcd.drawLine(X_CNTR_3, 19, x1, y1, DOTTED, FORCE) + lcd.drawFilledRectangle(x1 - 1, y1 - 1, 3, 3, ERASE) + lcd.drawFilledRectangle(x1 - 1, y1 - 1, 3, 3, SOLID) end end From 1aa30d07ddf7fb34d4841fdf03236d57e32806f7 Mon Sep 17 00:00:00 2001 From: teckel12 Date: Thu, 28 Sep 2017 10:52:33 -0400 Subject: [PATCH 8/9] Update comments to v1.1.1 --- README.md | 2 ++ iNav.lua | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index cb738d84..62aa502f 100644 --- a/README.md +++ b/README.md @@ -75,6 +75,8 @@ Audio feedback will play in background even if iNav LuaTelemetry screen is not d ## Release History +#### v1.1.1 - 09/28/2017 +* Refactored code to reduce memory #### v1.1.0 - 09/22/2017 * Repo moved to INAVFlight * Screen formatting for Taranis X9D, X9D+ & X9E diff --git a/iNav.lua b/iNav.lua index e46784c7..c0888d8f 100644 --- a/iNav.lua +++ b/iNav.lua @@ -1,4 +1,5 @@ --- Lua Telemetry Flight Status Screen for INAV/Taranis-v1.1 +-- Lua Telemetry Flight Status Screen for INAV/Taranis +-- Version: 1.1.1 -- Author: https://github.com/teckel12 -- Docs: https://github.com/iNavFlight/LuaTelemetry From ba217889fa7bba4a2e09c3cd8fe50c0bddaeae7f Mon Sep 17 00:00:00 2001 From: teckel12 Date: Thu, 28 Sep 2017 17:05:02 -0400 Subject: [PATCH 9/9] Fixed GPS info alignment and removed alititude graph on X9D, X9D+ & X9E --- README.md | 3 ++- iNav.lua | 20 ++++++++++---------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 62aa502f..4c5c4c61 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ * Launch-based model orientation and location indicators (great for lost orientation or if you lose site of your model) * Compass-based direction indicator -* Bar graphs for Fuel (% battery mAh capacity remaining), Battery voltage, RSSI strength, Tx battery (and Altitude for X9D, X9D+ & X9E transmitters) +* Bar graphs for Fuel (% battery mAh capacity remaining), Battery voltage, RSSI strength, Tx battery ~~(and Altitude for X9D, X9D+ & X9E transmitters)~~ * Display and speech notifications of flight modes and other flight status information (altitude hold, heading hold, etc.) * Speech notifications for % battery remaining (based on current), voltage low/critical, high altitude, lost GPS, ready to arm, armed, disarmed, etc. * GPS information: Satellites locked, GPS altitude, GPS coordinates @@ -77,6 +77,7 @@ Audio feedback will play in background even if iNav LuaTelemetry screen is not d #### v1.1.1 - 09/28/2017 * Refactored code to reduce memory +* Removed alititude graph for X9D, X9D+ & X9E transmitters (used too much memory?) #### v1.1.0 - 09/22/2017 * Repo moved to INAVFlight * Screen formatting for Taranis X9D, X9D+ & X9E diff --git a/iNav.lua b/iNav.lua index c0888d8f..91c5d79f 100644 --- a/iNav.lua +++ b/iNav.lua @@ -5,15 +5,15 @@ local WAVPATH = "/SCRIPTS/TELEMETRY/iNav/" local FLASH = INVERS + BLINK -local QX7 = (LCD_W<212) -local TIMER_POS = (QX7) and 60 or 150 +local QX7 = LCD_W < 212 +local TIMER_POS = QX7 and 60 or 150 local RXBATT_POS = LCD_W-17 -local RIGHT_POS = (QX7) and 129 or 195 -local GAUGE_WIDTH = (QX7) and 82 or 149 -local MODE_POS = (QX7) and 48 or 90 -local X_CNTR_1 = QX7 and 67 or 70 -local X_CNTR_2 = QX7 and 67 or 140 -local X_CNTR_3 = QX7 and 67 or 110 +local RIGHT_POS = QX7 and 129 or 213 -- 195 if alt +local GAUGE_WIDTH = QX7 and 82 or 166 -- 149 if alt +local MODE_POS = QX7 and 48 or 100 -- 90 if alt +local X_CNTR_1 = QX7 and 67 or 75 -- 70 if alt +local X_CNTR_2 = QX7 and 67 or 160 -- 140 if alt +local X_CNTR_3 = QX7 and 67 or 120 -- 110 if alt local modeIdPrev = false local armedPrev = false @@ -341,8 +341,8 @@ local function background() end local function gpsData(t, y, f) - lcd.drawText(85, 9, t, SMLSIZE) - local x = 85 + (RIGHT_POS-lcd.getLastPos()) + lcd.drawText(RIGHT_POS - 44, 9, t, SMLSIZE) + local x = RIGHT_POS - 44 + (RIGHT_POS-lcd.getLastPos()) lcd.drawText(x, y, t, SMLSIZE + f) end