From c8ce1def065998ce8673975371de3e783a0e4044 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Tue, 7 Feb 2017 23:22:11 +0100 Subject: [PATCH] reworked to split files + new QX7 version --- BFSetup.lua | 671 ---------------------------------------------- X7.lua | 66 +++++ X9.lua | 65 +++++ common/msp_sp.lua | 244 +++++++++++++++++ common/ui.lua | 403 ++++++++++++++++++++++++++++ 5 files changed, 778 insertions(+), 671 deletions(-) delete mode 100644 BFSetup.lua create mode 100644 X7.lua create mode 100644 X9.lua create mode 100644 common/msp_sp.lua create mode 100644 common/ui.lua diff --git a/BFSetup.lua b/BFSetup.lua deleted file mode 100644 index 17b9c430..00000000 --- a/BFSetup.lua +++ /dev/null @@ -1,671 +0,0 @@ --- --- MSP/SPORT code --- - --- Protocol version -SPORT_MSP_VERSION = bit32.lshift(1,5) - -SPORT_MSP_STARTFLAG = bit32.lshift(1,4) - --- Sensor ID used by the local LUA script -LOCAL_SENSOR_ID = 0x0D - --- Sensor ID used by the MSP server (BF, CF, MW, etc...) -REMOTE_SENSOR_ID = 0x1B - -REQUEST_FRAME_ID = 0x30 -REPLY_FRAME_ID = 0x32 - --- Sequence number for next MSP/SPORT packet -local sportMspSeq = 0 -local sportMspRemoteSeq = 0 - -local mspRxBuf = {} -local mspRxIdx = 1 -local mspRxCRC = 0 -local mspStarted = false -local mspLastReq = 0 - --- Stats -mspRequestsSent = 0 -mspRepliesReceived = 0 -mspPkRxed = 0 -mspErrorPk = 0 -mspStartPk = 0 -mspOutOfOrder = 0 -mspCRCErrors = 0 - -local function mspResetStats() - mspRequestsSent = 0 - mspRepliesReceived = 0 - mspPkRxed = 0 - mspErrorPk = 0 - mspStartPk = 0 - mspOutOfOrderPk = 0 - mspCRCErrors = 0 -end - -local mspTxBuf = {} -local mspTxIdx = 1 -local mspTxCRC = 0 - -local mspTxPk = 0 - -local function mspSendSport(payload) - - local dataId = 0 - dataId = payload[1] + bit32.lshift(payload[2],8) - - local value = 0 - value = payload[3] + bit32.lshift(payload[4],8) - + bit32.lshift(payload[5],16) + bit32.lshift(payload[6],24) - - local ret = sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value) - if ret then - mspTxPk = mspTxPk + 1 - end -end - -local function mspProcessTxQ() - - if (#(mspTxBuf) == 0) then - return false - end - - if not sportTelemetryPush() then - return true - end - - local payload = {} - payload[1] = sportMspSeq + SPORT_MSP_VERSION - sportMspSeq = bit32.band(sportMspSeq + 1, 0x0F) - - if mspTxIdx == 1 then - -- start flag - payload[1] = payload[1] + SPORT_MSP_STARTFLAG - end - - local i = 2 - while (i <= 6) do - payload[i] = mspTxBuf[mspTxIdx] - mspTxIdx = mspTxIdx + 1 - mspTxCRC = bit32.bxor(mspTxCRC,payload[i]) - i = i + 1 - if mspTxIdx > #(mspTxBuf) then - break - end - end - - if i <= 6 then - payload[i] = mspTxCRC - i = i + 1 - - -- zero fill - while i <= 6 do - payload[i] = 0 - i = i + 1 - end - - mspSendSport(payload) - - mspTxBuf = {} - mspTxIdx = 1 - mspTxCRC = 0 - - return false - end - - mspSendSport(payload) - return true -end - -local function mspSendRequest(cmd,payload) - - -- busy - if #(mspTxBuf) ~= 0 then - return nil - end - - mspTxBuf[1] = #(payload) - mspTxBuf[2] = bit32.band(cmd,0xFF) -- MSP command - - for i=1,#(payload) do - mspTxBuf[i+2] = payload[i] - end - - mspLastReq = cmd - mspRequestsSent = mspRequestsSent + 1 - return mspProcessTxQ() -end - -local function mspReceivedReply(payload) - - mspPkRxed = mspPkRxed + 1 - - local idx = 1 - local head = payload[idx] - local err_flag = (bit32.band(head,0x20) ~= 0) - idx = idx + 1 - - if err_flag then - -- error flag set - mspStarted = false - - mspErrorPk = mspErrorPk + 1 - - -- return error - -- CRC checking missing - - --return payload[idx] - return nil - end - - local start = (bit32.band(head,0x10) ~= 0) - local seq = bit32.band(head,0x0F) - - if start then - -- start flag set - mspRxIdx = 1 - mspRxBuf = {} - - mspRxSize = payload[idx] - mspRxCRC = bit32.bxor(mspRxSize,mspLastReq) - idx = idx + 1 - mspStarted = true - - mspStartPk = mspStartPk + 1 - - elseif not mspStarted then - mspOutOfOrder = mspOutOfOrder + 1 - return nil - - elseif bit32.band(sportMspRemoteSeq + 1, 0x0F) ~= seq then - mspOutOfOrder = mspOutOfOrder + 1 - mspStarted = false - return nil - end - - while (idx <= 6) and (mspRxIdx <= mspRxSize) do - mspRxBuf[mspRxIdx] = payload[idx] - mspRxCRC = bit32.bxor(mspRxCRC,payload[idx]) - mspRxIdx = mspRxIdx + 1 - idx = idx + 1 - end - - if idx > 6 then - sportMspRemoteSeq = seq - return true - end - - -- check CRC - if mspRxCRC ~= payload[idx] then - mspStarted = false - mspCRCErrors = mspCRCErrors + 1 - return nil - end - - mspRepliesReceived = mspRepliesReceived + 1 - mspStarted = false - return mspRxBuf -end - -local function mspPollReply() - while true do - local sensorId, frameId, dataId, value = sportTelemetryPop() - if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then - - local payload = {} - payload[1] = bit32.band(dataId,0xFF) - dataId = bit32.rshift(dataId,8) - payload[2] = bit32.band(dataId,0xFF) - - payload[3] = bit32.band(value,0xFF) - value = bit32.rshift(value,8) - payload[4] = bit32.band(value,0xFF) - value = bit32.rshift(value,8) - payload[5] = bit32.band(value,0xFF) - value = bit32.rshift(value,8) - payload[6] = bit32.band(value,0xFF) - - local ret = mspReceivedReply(payload) - if type(ret) == "table" then - return mspLastReq,ret - end - else - break - end - end - - return nil -end - --- --- End of MSP/SPORT code --- - --- getter -local MSP_RC_TUNING = 111 -local MSP_PID = 112 - --- setter -local MSP_SET_PID = 202 -local MSP_SET_RC_TUNING = 204 - --- BF specials -local MSP_PID_ADVANCED = 94 -local MSP_SET_PID_ADVANCED = 95 -local MSP_VTX_CONFIG = 88 -local MSP_VTX_SET_CONFIG = 89 - -local MSP_EEPROM_WRITE = 250 - -local REQ_TIMEOUT = 80 -- 800ms request timeout - ---local PAGE_REFRESH = 1 -local PAGE_DISPLAY = 2 -local EDITING = 3 -local PAGE_SAVING = 4 -local MENU_DISP = 5 - -local gState = PAGE_DISPLAY - -local function postReadVTX(page) - if page.values[1] == 3 then -- SmartAudio - page.fields[3].table = { 25, 200, 500, 800 } - page.fields[3].max = 4 - elseif page.values[1] == 4 then -- Tramp - page.fields[3].table = { 25, 100, 200, 400, 600 } - page.fields[3].max = 5 - else - -- TODO: print label on unavailable (0xFF) vs. unsupported (0) - --page.values = nil - end -end - -local function getWriteValuesVTX(values) - local channel = (values[2]-1)*8 + values[3]-1 - return { bit32.band(channel,0xFF), bit32.rshift(channel,8), values[4], values[5] } -end - -local SetupPages = { - { - title = "PIDs", - text = { - { t = "Roll", x = 21, y = 14 }, - { t = "Pitch", x = 75, y = 14 }, - { t = "Yaw", x = 129, y = 14 }, - }, - fields = { - -- ROLL - { t = "P", x = 25, y = 24, i=1 }, - { t = "I", x = 25, y = 34, i=2 }, - { t = "D", x = 25, y = 44, i=3 }, - -- PITCH - { t = "P", x = 80, y = 24, i=4 }, - { t = "I", x = 80, y = 34, i=5 }, - { t = "D", x = 80, y = 44, i=6 }, - -- YAW - { t = "P", x = 135, y = 24, i=7 }, - { t = "I", x = 135, y = 34, i=8 }, - --{ t = "D", x = 135, y = 44, i=9 }, - }, - read = MSP_PID, - write = MSP_SET_PID, - }, - { - title = "Rates", - text = { - { t = "Super rates", x = 5, y = 14 }, - { t = "RC", x = 80, y = 14 } - }, - fields = { - -- Super Rate - { t = "Roll", x = 10, y = 24, sp = 40, i=3 }, - { t = "Pitch", x = 10, y = 34, sp = 40, i=4 }, - - -- Roll + Pitch - { t = "Rate", x = 75, y = 29, sp = 30, i=1 }, - { t = "Expo", x = 135, y = 29, sp = 30, i=2 }, - - -- Yaw - { t = "Yaw", x = 10, y = 48, sp = 40, i=5 }, - { t = "Rate", x = 75, y = 48, sp = 30, i=12 }, - { t = "Expo", x = 135, y = 48, sp = 30, i=11 }, - }, - read = MSP_RC_TUNING, - write = MSP_SET_RC_TUNING, - }, - { - title = "VTX", - text = {}, - fields = { - -- Super Rate - { t = "Band", x = 25, y = 14, sp = 50, i=2, min=1, max=5, table = { "A", "B", "E", "F", "R" } }, - { t = "Channel", x = 25, y = 24, sp = 50, i=3, min=1, max=8 }, - { t = "Power", x = 25, y = 34, sp = 50, i=4, min=1 }, - { t = "Pit", x = 25, y = 44, sp = 50, i=5, min=0, max=1, table = { [0]="OFF", "ON" } }, - { t = "(Dev)", x = 100, y = 14, sp = 32, i=1, ro=true, table = {[3]="SmartAudio",[4]="Tramp"} }, - }, - read = MSP_VTX_CONFIG, - write = MSP_VTX_SET_CONFIG, - postRead = postReadVTX, - getWriteValues = getWriteValuesVTX, - saveMaxRetries = 0, - saveTimeout = 200 -- 2s - } -} - -local currentPage = 1 -local currentLine = 1 -local saveTS = 0 -local saveTimeout = 0 -local saveRetries = 0 -local saveMaxRetries = 0 - -local function saveSettings(new) - local page = SetupPages[currentPage] - if page.values then - if page.getWriteValues then - mspSendRequest(page.write,page.getWriteValues(page.values)) - else - mspSendRequest(page.write,page.values) - end - saveTS = getTime() - if gState == PAGE_SAVING then - saveRetries = saveRetries + 1 - else - gState = PAGE_SAVING - saveRetries = 0 - saveMaxRetries = page.saveMaxRetries or 2 -- default 2 - saveTimeout = page.saveTimeout or 150 -- default 1.5s - end - end -end - -local function invalidatePages() - for i=1,#(SetupPages) do - local page = SetupPages[i] - page.values = nil - end - gState = PAGE_DISPLAY - saveTS = 0 -end - -local menuList = { - - { t = "save page", - f = saveSettings }, - - { t = "reload", - f = invalidatePages } -} - -local telemetryScreenActive = false -local menuActive = false - -local function processMspReply(cmd,rx_buf) - - if cmd == nil or rx_buf == nil then - return - end - - local page = SetupPages[currentPage] - - -- ignore replies to write requests for now - if cmd == page.write then - if cmd ~= MSP_VTX_SET_CONFIG then - mspSendRequest(MSP_EEPROM_WRITE,{}) - end - return - end - - if cmd == MSP_EEPROM_WRITE then - gState = PAGE_DISPLAY - page.values = nil - saveTS = 0 - return - end - - if cmd ~= page.read then - return - end - - if #(rx_buf) > 0 then - page.values = {} - for i=1,#(rx_buf) do - page.values[i] = rx_buf[i] - end - - if page.postRead ~= nil then - page.postRead(page) - end - end -end - -local function MaxLines() - return #(SetupPages[currentPage].fields) -end - -local function incPage(inc) - currentPage = currentPage + inc - if currentPage > #(SetupPages) then - currentPage = 1 - elseif currentPage < 1 then - currentPage = #(SetupPages) - end - currentLine = 1 -end - -local function incLine(inc) - currentLine = currentLine + inc - if currentLine > MaxLines() then - currentLine = 1 - elseif currentLine < 1 then - currentLine = MaxLines() - end -end - -local function incMenu(inc) - menuActive = menuActive + inc - if menuActive > #(menuList) then - menuActive = 1 - elseif menuActive < 1 then - menuActive = #(menuList) - end -end - -local function requestPage(page) - if page.read and ((page.reqTS == nil) or (page.reqTS + REQ_TIMEOUT <= getTime())) then - page.reqTS = getTime() - mspSendRequest(page.read,{}) - end -end - -local function drawScreen(page,page_locked) - - local screen_title = page.title - - lcd.drawScreenTitle('Betaflight setup: '..screen_title,currentPage,#(SetupPages)) - - for i=1,#(page.text) do - local f = page.text[i] - lcd.drawText(f.x, f.y, f.t, text_options) - end - - for i=1,#(page.fields) do - local f = page.fields[i] - - local text_options = 0 - if i == currentLine then - text_options = INVERS - if gState == EDITING then - text_options = text_options + BLINK - end - end - - lcd.drawText(f.x, f.y, f.t .. ":", 0) - - -- draw some value - local spacing = 20 - if f.sp ~= nil then - spacing = f.sp - end - - local idx = f.i or i - if page.values and page.values[idx] then - local val = page.values[idx] - if f.table and f.table[page.values[idx]] then - val = f.table[page.values[idx]] - end - lcd.drawText(f.x + spacing, f.y, val, text_options) - else - lcd.drawText(f.x + spacing, f.y, "---", text_options) - end - end -end - -local function clipValue(val,min,max) - if val < min then - val = min - elseif val > max then - val = max - end - - return val -end - -local function getCurrentField() - local page = SetupPages[currentPage] - return page.fields[currentLine] -end - -local function incValue(inc) - local page = SetupPages[currentPage] - local field = page.fields[currentLine] - local idx = field.i or currentLine - page.values[idx] = clipValue(page.values[idx] + inc, field.min or 0, field.max or 255) -end - -local function drawMenu() - local x = 40 - local y = 12 - local w = 120 - local h = #(menuList) * 8 + 6 - lcd.drawFilledRectangle(x,y,w,h,ERASE) - lcd.drawRectangle(x,y,w-1,h-1,SOLID) - lcd.drawText(x+4,y+3,"Menu:") - - for i,e in ipairs(menuList) do - if menuActive == i then - lcd.drawText(x+36,y+(i-1)*8+3,e.t,INVERS) - else - lcd.drawText(x+36,y+(i-1)*8+3,e.t) - end - end -end - -local EVT_MENU_LONG = bit32.bor(bit32.band(EVT_MENU_BREAK,0x1f),0x80) - -local lastRunTS = 0 - -local function run(event) - - local now = getTime() - - -- if lastRunTS old than 500ms - if lastRunTS + 50 < now then - invalidatePages() - end - lastRunTS = now - - if (gState == PAGE_SAVING) and (saveTS + saveTimeout < now) then - if saveRetries < saveMaxRetries then - saveSettings() - else - -- max retries reached - gState = PAGE_DISPLAY - invalidatePages() - end - end - - if #(mspTxBuf) > 0 then - mspProcessTxQ() - end - - -- navigation - if event == EVT_MENU_LONG then - menuActive = 1 - gState = MENU_DISP - - -- menu is currently displayed - elseif gState == MENU_DISP then - if event == EVT_EXIT_BREAK then - gState = PAGE_DISPLAY - elseif event == EVT_PLUS_BREAK or event == EVT_ROT_LEFT then - incMenu(-1) - elseif event == EVT_MINUS_BREAK or event == EVT_ROT_RIGHT then - incMenu(1) - elseif event == EVT_ENTER_BREAK then - gState = PAGE_DISPLAY - menuList[menuActive].f() - end - -- normal page viewing - elseif gState <= PAGE_DISPLAY then - if event == EVT_MENU_BREAK then - incPage(1) - elseif event == EVT_PLUS_BREAK or event == EVT_ROT_LEFT then - incLine(-1) - elseif event == EVT_MINUS_BREAK or event == EVT_ROT_RIGHT then - incLine(1) - elseif event == EVT_ENTER_BREAK then - local page = SetupPages[currentPage] - local field = page.fields[currentLine] - local idx = field.i or currentLine - if page.values and page.values[idx] and (field.ro ~= true) then - gState = EDITING - end - end - -- editing value - elseif gState == EDITING then - if (event == EVT_EXIT_BREAK) or (event == EVT_ENTER_BREAK) then - gState = PAGE_DISPLAY - elseif event == EVT_PLUS_FIRST or event == EVT_PLUS_REPT or event == EVT_ROT_RIGHT then - incValue(1) - elseif event == EVT_MINUS_FIRST or event == EVT_MINUS_REPT or event == EVT_ROT_LEFT then - incValue(-1) - end - end - - local page = SetupPages[currentPage] - local page_locked = false - - if not page.values then - -- request values - requestPage(page) - page_locked = true - end - - -- draw screen - lcd.clear() - drawScreen(page,page_locked) - - -- do we have valid telemetry data? - if getValue("RSSI") == 0 then - -- No! - lcd.drawText(70,55,"No telemetry",BLINK) - invalidatePages() - end - - if gState == MENU_DISP then - drawMenu() - elseif gState == PAGE_SAVING then - lcd.drawFilledRectangle(40,12,120,30,ERASE) - lcd.drawRectangle(40,12,120,30,SOLID) - lcd.drawText(44,15,"Saving...",DBLSIZE + BLINK) - end - - processMspReply(mspPollReply()) - return 0 -end - -return {run=run} diff --git a/X7.lua b/X7.lua new file mode 100644 index 00000000..b0bb50db --- /dev/null +++ b/X7.lua @@ -0,0 +1,66 @@ +SetupPages = { + { + title = "PIDs", + text = { + { t = "Roll", x = 1, y = 12 }, + { t = "Pitch", x = 43, y = 12 }, + { t = "Yaw", x = 85, y = 12 }, + }, + fields = { + -- ROLL + { t = "P", x = 1, y = 24, i=1 }, + { t = "I", x = 1, y = 34, i=2 }, + { t = "D", x = 1, y = 44, i=3 }, + -- PITCH + { t = "P", x = 43, y = 24, i=4 }, + { t = "I", x = 43, y = 34, i=5 }, + { t = "D", x = 43, y = 44, i=6 }, + -- YAW + { t = "P", x = 85, y = 24, i=7 }, + { t = "I", x = 85, y = 34, i=8 }, + --{ t = "D", x = 85, y = 44, i=9 }, + }, + }, + { + title = "Rates", + text = { + { t = "S-Rates", x = 8, y = 12 }, + { t = "RC", x = 62, y = 12 }, + { t = "Expo", x = 86, y = 12 }, + }, + fields = { + -- Super Rate + { t = "R", x = 1, y = 24, i=3 }, + { t = "P", x = 1, y = 34, i=4 }, + + -- Roll + Pitch + { x = 60, y = 29, i=1 }, + { x = 90, y = 29, i=2 }, + + -- Yaw + { t = "Y", x = 1, y = 44, i=5 }, + { x = 60, y = 44, i=12 }, + { x = 90, y = 44, i=11 }, + }, + }, + { + title = "VTX", + text = {}, + fields = { + -- Super Rate + { t = "Band", x = 1, y = 12, sp = 34, i=2, min=1, max=5, table = { "A", "B", "E", "F", "R" } }, + { t = "Ch", x = 1, y = 22, sp = 34, i=3, min=1, max=8 }, + { t = "Pw", x = 1, y = 32, sp = 34, i=4, min=1 }, + { t = "Pit", x = 1, y = 42, sp = 34, i=5, min=0, max=1, table = { [0]="OFF", "ON" } }, + { t = "Dev", x = 60, y = 12, sp = 34, i=1, ro=true, table = {[3]="SA",[4]="Tramp"} }, + { t = "Freq", x = 60, y = 22, sp = 34, i="f", ro=true }, + }, + } +} + +MenuBox = { x=1, y=10, w=100, x_offset=36, h_line=10, h_offset=3 } +SaveBox = { x=20, y=12, w=100, x_offset=4, h=30, h_offset=5 } +NoTelem = { 36, 55, "No Telemetry", BLINK } + +local run_ui = assert(loadScript("/SCRIPTS/BF/ui.lua"))() +return {run=run_ui} diff --git a/X9.lua b/X9.lua new file mode 100644 index 00000000..989e7151 --- /dev/null +++ b/X9.lua @@ -0,0 +1,65 @@ +SetupPages = { + { + title = "PIDs", + text = { + { t = "Roll", x = 21, y = 14 }, + { t = "Pitch", x = 75, y = 14 }, + { t = "Yaw", x = 129, y = 14 }, + }, + fields = { + -- ROLL + { t = "P", x = 25, y = 24 }, + { t = "I", x = 25, y = 34 }, + { t = "D", x = 25, y = 44 }, + -- PITCH + { t = "P", x = 80, y = 24 }, + { t = "I", x = 80, y = 34 }, + { t = "D", x = 80, y = 44 }, + -- YAW + { t = "P", x = 135, y = 24 }, + { t = "I", x = 135, y = 34 }, + --{ t = "D", x = 135, y = 44, i=9 }, + }, + }, + { + title = "Rates", + text = { + { t = "Super rates", x = 5, y = 14 }, + { t = "RC", x = 80, y = 14 } + }, + fields = { + -- Super Rate + { t = "Roll", x = 10, y = 24, sp = 40, i=3 }, + { t = "Pitch", x = 10, y = 34, sp = 40, i=4 }, + + -- Roll + Pitch + { t = "Rate", x = 75, y = 29, sp = 30, i=1 }, + { t = "Expo", x = 135, y = 29, sp = 30, i=2 }, + + -- Yaw + { t = "Yaw", x = 10, y = 48, sp = 40, i=5 }, + { t = "Rate", x = 75, y = 48, sp = 30, i=12 }, + { t = "Expo", x = 135, y = 48, sp = 30, i=11 }, + }, + }, + { + title = "VTX", + text = {}, + fields = { + -- Super Rate + { t = "Band", x = 25, y = 14, sp = 50, i=2, min=1, max=5, table = { "A", "B", "E", "F", "R" } }, + { t = "Channel", x = 25, y = 24, sp = 50, i=3, min=1, max=8 }, + { t = "Power", x = 25, y = 34, sp = 50, i=4, min=1 }, + { t = "Pit", x = 25, y = 44, sp = 50, i=5, min=0, max=1, table = { [0]="OFF", "ON" } }, + { t = "Dev", x = 100, y = 14, sp = 32, i=1, ro=true, table = {[3]="SmartAudio",[4]="Tramp"} }, + { t = "Freq", x = 100, y = 24, sp = 32, i="f", ro=true }, + }, + } +} + +MenuBox = { x=40, y=12, w=120, x_offset=36, h_line=8, h_offset=3 } +SaveBox = { x=40, y=12, w=120, x_offset=4, h=30, h_offset=5 } +NoTelem = { 70, 55, "No Telemetry", BLINK } + +local run_ui = assert(loadScript("/SCRIPTS/BF/ui.lua"))() +return {run=run_ui} diff --git a/common/msp_sp.lua b/common/msp_sp.lua new file mode 100644 index 00000000..673e7077 --- /dev/null +++ b/common/msp_sp.lua @@ -0,0 +1,244 @@ +-- +-- MSP/SPORT code +-- + +-- Protocol version +SPORT_MSP_VERSION = bit32.lshift(1,5) + +SPORT_MSP_STARTFLAG = bit32.lshift(1,4) + +-- Sensor ID used by the local LUA script +LOCAL_SENSOR_ID = 0x0D + +-- Sensor ID used by the MSP server (BF, CF, MW, etc...) +REMOTE_SENSOR_ID = 0x1B + +REQUEST_FRAME_ID = 0x30 +REPLY_FRAME_ID = 0x32 + +-- Sequence number for next MSP/SPORT packet +local sportMspSeq = 0 +local sportMspRemoteSeq = 0 + +local mspRxBuf = {} +local mspRxIdx = 1 +local mspRxCRC = 0 +local mspStarted = false +local mspLastReq = 0 + +-- Stats +mspRequestsSent = 0 +mspRepliesReceived = 0 +mspPkRxed = 0 +mspErrorPk = 0 +mspStartPk = 0 +mspOutOfOrder = 0 +mspCRCErrors = 0 + +local function mspResetStats() + mspRequestsSent = 0 + mspRepliesReceived = 0 + mspPkRxed = 0 + mspErrorPk = 0 + mspStartPk = 0 + mspOutOfOrderPk = 0 + mspCRCErrors = 0 +end + +local mspTxBuf = {} +local mspTxIdx = 1 +local mspTxCRC = 0 + +local mspTxPk = 0 + +local function mspSendSport(payload) + + local dataId = 0 + dataId = payload[1] + bit32.lshift(payload[2],8) + + local value = 0 + value = payload[3] + bit32.lshift(payload[4],8) + + bit32.lshift(payload[5],16) + bit32.lshift(payload[6],24) + + local ret = sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value) + if ret then + mspTxPk = mspTxPk + 1 + end +end + +function mspProcessTxQ() + + if (#(mspTxBuf) == 0) then + return false + end + + if not sportTelemetryPush() then + return true + end + + local payload = {} + payload[1] = sportMspSeq + SPORT_MSP_VERSION + sportMspSeq = bit32.band(sportMspSeq + 1, 0x0F) + + if mspTxIdx == 1 then + -- start flag + payload[1] = payload[1] + SPORT_MSP_STARTFLAG + end + + local i = 2 + while (i <= 6) do + payload[i] = mspTxBuf[mspTxIdx] + mspTxIdx = mspTxIdx + 1 + mspTxCRC = bit32.bxor(mspTxCRC,payload[i]) + i = i + 1 + if mspTxIdx > #(mspTxBuf) then + break + end + end + + if i <= 6 then + payload[i] = mspTxCRC + i = i + 1 + + -- zero fill + while i <= 6 do + payload[i] = 0 + i = i + 1 + end + + mspSendSport(payload) + + mspTxBuf = {} + mspTxIdx = 1 + mspTxCRC = 0 + + return false + end + + mspSendSport(payload) + return true +end + +function mspSendRequest(cmd,payload) + + -- busy + if #(mspTxBuf) ~= 0 then + return nil + end + + mspTxBuf[1] = #(payload) + mspTxBuf[2] = bit32.band(cmd,0xFF) -- MSP command + + for i=1,#(payload) do + mspTxBuf[i+2] = payload[i] + end + + mspLastReq = cmd + mspRequestsSent = mspRequestsSent + 1 + return mspProcessTxQ() +end + +local function mspReceivedReply(payload) + + mspPkRxed = mspPkRxed + 1 + + local idx = 1 + local head = payload[idx] + local err_flag = (bit32.band(head,0x20) ~= 0) + idx = idx + 1 + + if err_flag then + -- error flag set + mspStarted = false + + mspErrorPk = mspErrorPk + 1 + + -- return error + -- CRC checking missing + + --return payload[idx] + return nil + end + + local start = (bit32.band(head,0x10) ~= 0) + local seq = bit32.band(head,0x0F) + + if start then + -- start flag set + mspRxIdx = 1 + mspRxBuf = {} + + mspRxSize = payload[idx] + mspRxCRC = bit32.bxor(mspRxSize,mspLastReq) + idx = idx + 1 + mspStarted = true + + mspStartPk = mspStartPk + 1 + + elseif not mspStarted then + mspOutOfOrder = mspOutOfOrder + 1 + return nil + + elseif bit32.band(sportMspRemoteSeq + 1, 0x0F) ~= seq then + mspOutOfOrder = mspOutOfOrder + 1 + mspStarted = false + return nil + end + + while (idx <= 6) and (mspRxIdx <= mspRxSize) do + mspRxBuf[mspRxIdx] = payload[idx] + mspRxCRC = bit32.bxor(mspRxCRC,payload[idx]) + mspRxIdx = mspRxIdx + 1 + idx = idx + 1 + end + + if idx > 6 then + sportMspRemoteSeq = seq + return true + end + + -- check CRC + if mspRxCRC ~= payload[idx] then + mspStarted = false + mspCRCErrors = mspCRCErrors + 1 + return nil + end + + mspRepliesReceived = mspRepliesReceived + 1 + mspStarted = false + return mspRxBuf +end + +function mspPollReply() + while true do + local sensorId, frameId, dataId, value = sportTelemetryPop() + if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then + + local payload = {} + payload[1] = bit32.band(dataId,0xFF) + dataId = bit32.rshift(dataId,8) + payload[2] = bit32.band(dataId,0xFF) + + payload[3] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[4] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[5] = bit32.band(value,0xFF) + value = bit32.rshift(value,8) + payload[6] = bit32.band(value,0xFF) + + local ret = mspReceivedReply(payload) + if type(ret) == "table" then + return mspLastReq,ret + end + else + break + end + end + + return nil +end + +-- +-- End of MSP/SPORT code +-- diff --git a/common/ui.lua b/common/ui.lua new file mode 100644 index 00000000..514aa680 --- /dev/null +++ b/common/ui.lua @@ -0,0 +1,403 @@ +-- load msp.lua +assert(loadScript("/SCRIPTS/BF/msp_sp.lua"))() + +-- getter +local MSP_RC_TUNING = 111 +local MSP_PID = 112 + +-- setter +local MSP_SET_PID = 202 +local MSP_SET_RC_TUNING = 204 + +-- BF specials +local MSP_PID_ADVANCED = 94 +local MSP_SET_PID_ADVANCED = 95 +local MSP_VTX_CONFIG = 88 +local MSP_VTX_SET_CONFIG = 89 + +local MSP_EEPROM_WRITE = 250 + +local REQ_TIMEOUT = 80 -- 800ms request timeout + +--local PAGE_REFRESH = 1 +local PAGE_DISPLAY = 2 +local EDITING = 3 +local PAGE_SAVING = 4 +local MENU_DISP = 5 + +local gState = PAGE_DISPLAY + +local currentPage = 1 +local currentLine = 1 +local saveTS = 0 +local saveTimeout = 0 +local saveRetries = 0 +local saveMaxRetries = 0 + +local function saveSettings(new) + local page = SetupPages[currentPage] + if page.values then + if page.getWriteValues then + mspSendRequest(page.write,page.getWriteValues(page.values)) + else + mspSendRequest(page.write,page.values) + end + saveTS = getTime() + if gState == PAGE_SAVING then + saveRetries = saveRetries + 1 + else + gState = PAGE_SAVING + saveRetries = 0 + saveMaxRetries = page.saveMaxRetries or 2 -- default 2 + saveTimeout = page.saveTimeout or 150 -- default 1.5s + end + end +end + +local function invalidatePages() + for i=1,#(SetupPages) do + local page = SetupPages[i] + page.values = nil + end + gState = PAGE_DISPLAY + saveTS = 0 +end + +local menuList = { + + { t = "save page", + f = saveSettings }, + + { t = "reload", + f = invalidatePages } +} + +local telemetryScreenActive = false +local menuActive = false + +local function processMspReply(cmd,rx_buf) + + if cmd == nil or rx_buf == nil then + return + end + + local page = SetupPages[currentPage] + + -- ignore replies to write requests for now + if cmd == page.write then + if cmd ~= MSP_VTX_SET_CONFIG then + mspSendRequest(MSP_EEPROM_WRITE,{}) + end + return + end + + if cmd == MSP_EEPROM_WRITE then + gState = PAGE_DISPLAY + page.values = nil + saveTS = 0 + return + end + + if cmd ~= page.read then + return + end + + if #(rx_buf) > 0 then + page.values = {} + for i=1,#(rx_buf) do + page.values[i] = rx_buf[i] + end + + if page.postRead ~= nil then + page.postRead(page) + end + end +end + +local function MaxLines() + return #(SetupPages[currentPage].fields) +end + +local function incPage(inc) + currentPage = currentPage + inc + if currentPage > #(SetupPages) then + currentPage = 1 + elseif currentPage < 1 then + currentPage = #(SetupPages) + end + currentLine = 1 +end + +local function incLine(inc) + currentLine = currentLine + inc + if currentLine > MaxLines() then + currentLine = 1 + elseif currentLine < 1 then + currentLine = MaxLines() + end +end + +local function incMenu(inc) + menuActive = menuActive + inc + if menuActive > #(menuList) then + menuActive = 1 + elseif menuActive < 1 then + menuActive = #(menuList) + end +end + +local function requestPage(page) + if page.read and ((page.reqTS == nil) or (page.reqTS + REQ_TIMEOUT <= getTime())) then + page.reqTS = getTime() + mspSendRequest(page.read,{}) + end +end + +local function drawScreen(page,page_locked) + + local screen_title = page.title + + lcd.drawText(1,1,"Betaflight / "..screen_title,INVERS) + + for i=1,#(page.text) do + local f = page.text[i] + lcd.drawText(f.x, f.y, f.t, text_options) + end + + for i=1,#(page.fields) do + local f = page.fields[i] + + local text_options = 0 + if i == currentLine then + text_options = INVERS + if gState == EDITING then + text_options = text_options + BLINK + end + end + + local spacing = 20 + + if f.t ~= nil then + lcd.drawText(f.x, f.y, f.t .. ":", 0) + + -- draw some value + if f.sp ~= nil then + spacing = f.sp + end + else + spacing = 0 + end + + local idx = f.i or i + if page.values and page.values[idx] then + local val = page.values[idx] + if f.table and f.table[page.values[idx]] then + val = f.table[page.values[idx]] + end + lcd.drawText(f.x + spacing, f.y, val, text_options) + else + lcd.drawText(f.x + spacing, f.y, "---", text_options) + end + end +end + +local function clipValue(val,min,max) + if val < min then + val = min + elseif val > max then + val = max + end + + return val +end + +local function getCurrentField() + local page = SetupPages[currentPage] + return page.fields[currentLine] +end + +local function incValue(inc) + local page = SetupPages[currentPage] + local field = page.fields[currentLine] + local idx = field.i or currentLine + page.values[idx] = clipValue(page.values[idx] + inc, field.min or 0, field.max or 255) + if field.upd then field.upd(page) end +end + +local function drawMenu() + local x = MenuBox.x + local y = MenuBox.y + local w = MenuBox.w + local h_line = MenuBox.h_line + local h_offset = MenuBox.h_offset + local h = #(menuList) * h_line + h_offset*2 + lcd.drawFilledRectangle(x,y,w,h,ERASE) + lcd.drawRectangle(x,y,w-1,h-1,SOLID) + lcd.drawText(x+h_line/2,y+h_offset,"Menu:") + + for i,e in ipairs(menuList) do + if menuActive == i then + lcd.drawText(x+MenuBox.x_offset,y+(i-1)*h_line+h_offset,e.t,INVERS) + else + lcd.drawText(x+MenuBox.x_offset,y+(i-1)*h_line+h_offset,e.t) + end + end +end + +local lastRunTS = 0 +local killEnterBreak = 0 + +local function run_ui(event) + + local now = getTime() + + -- if lastRunTS old than 500ms + if lastRunTS + 50 < now then + invalidatePages() + end + lastRunTS = now + + if (gState == PAGE_SAVING) and (saveTS + saveTimeout < now) then + if saveRetries < saveMaxRetries then + saveSettings() + else + -- max retries reached + gState = PAGE_DISPLAY + invalidatePages() + end + end + + -- process send queue + mspProcessTxQ() + + -- navigation + if (event == EVT_MENU_LONG) then + menuActive = 1 + gState = MENU_DISP + -- menu is currently displayed + elseif gState == MENU_DISP then + if event == EVT_EXIT_BREAK then + gState = PAGE_DISPLAY + elseif event == EVT_PLUS_BREAK or event == EVT_ROT_LEFT then + incMenu(-1) + elseif event == EVT_MINUS_BREAK or event == EVT_ROT_RIGHT then + incMenu(1) + elseif event == EVT_ENTER_BREAK then + gState = PAGE_DISPLAY + menuList[menuActive].f() + end + -- normal page viewing + elseif gState <= PAGE_DISPLAY then + if event == EVT_MENU_BREAK then + incPage(1) + elseif event == EVT_PLUS_BREAK or event == EVT_ROT_LEFT then + incLine(-1) + elseif event == EVT_MINUS_BREAK or event == EVT_ROT_RIGHT then + incLine(1) + elseif event == EVT_ENTER_BREAK then + local page = SetupPages[currentPage] + local field = page.fields[currentLine] + local idx = field.i or currentLine + if page.values and page.values[idx] and (field.ro ~= true) then + gState = EDITING + end + end + -- editing value + elseif gState == EDITING then + if (event == EVT_EXIT_BREAK) or (event == EVT_ENTER_BREAK) then + gState = PAGE_DISPLAY + elseif event == EVT_PLUS_FIRST or event == EVT_PLUS_REPT or event == EVT_ROT_RIGHT then + incValue(1) + elseif event == EVT_MINUS_FIRST or event == EVT_MINUS_REPT or event == EVT_ROT_LEFT then + incValue(-1) + end + end + + local page = SetupPages[currentPage] + local page_locked = false + + if not page.values then + -- request values + requestPage(page) + page_locked = true + end + + -- draw screen + lcd.clear() + drawScreen(page,page_locked) + + -- do we have valid telemetry data? + if getValue("RSSI") == 0 then + -- No! + lcd.drawText(NoTelem[1],NoTelem[2],NoTelem[3],NoTelem[4]) + invalidatePages() + end + + if gState == MENU_DISP then + drawMenu() + elseif gState == PAGE_SAVING then + lcd.drawFilledRectangle(SaveBox.x,SaveBox.y,SaveBox.w,SaveBox.h,ERASE) + lcd.drawRectangle(SaveBox.x,SaveBox.y,SaveBox.w,SaveBox.h,SOLID) + lcd.drawText(SaveBox.x+SaveBox.x_offset,SaveBox.y+SaveBox.h_offset,"Saving...",DBLSIZE + BLINK) + end + + processMspReply(mspPollReply()) + return 0 +end + +local freqLookup = { + { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725 }, -- Boscam A + { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866 }, -- Boscam B + { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945 }, -- Boscam E + { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880 }, -- FatShark + { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917 }, -- RaceBand +} + +local function updateVTXFreq(page) + page.values["f"] = freqLookup[page.values[2]][page.values[3]] +end + +local function postReadVTX(page) + if page.values[1] == 3 then -- SmartAudio + page.fields[3].table = { 25, 200, 500, 800 } + page.fields[3].max = 4 + elseif page.values[1] == 4 then -- Tramp + page.fields[3].table = { 25, 100, 200, 400, 600 } + page.fields[3].max = 5 + else + -- TODO: print label on unavailable (0xFF) vs. unsupported (0) + --page.values = nil + end + + + if page.values and page.values[2] and page.values[3] then + if page.values[2] > 0 and page.values[3] > 0 then + updateVTXFreq(page) + else + page.values = nil + end + end +end + +local function getWriteValuesVTX(values) + local channel = (values[2]-1)*8 + values[3]-1 + return { bit32.band(channel,0xFF), bit32.rshift(channel,8), values[4], values[5] } +end + +SetupPages[1].read = MSP_PID +SetupPages[1].write = MSP_SET_PID + +SetupPages[2].read = MSP_RC_TUNING +SetupPages[2].write = MSP_SET_RC_TUNING + +SetupPages[3].read = MSP_VTX_CONFIG +SetupPages[3].write = MSP_VTX_SET_CONFIG +SetupPages[3].postRead = postReadVTX +SetupPages[3].getWriteValues = getWriteValuesVTX +SetupPages[3].saveMaxRetries = 0 +SetupPages[3].saveTimeout = 300 -- 3s + +SetupPages[3].fields[1].upd = updateVTXFreq +SetupPages[3].fields[2].upd = updateVTXFreq + +return run_ui