init of lua script for tango

EMU/emu.lua

@@ -0,0 +1,14 @@
+SCRIPT_HOME = "/SCRIPTS/EMU"
+
+apiVersion = 0
+
+protocol = assert(loadScript(SCRIPT_HOME.."/protocols.lua"))()
+radio = assert(loadScript(SCRIPT_HOME.."/radios.lua"))()
+
+assert(loadScript(radio.preLoad))()
+assert(loadScript(protocol.transport))()
+assert(loadScript(SCRIPT_HOME.."/MSP/common.lua"))()
+
+local run_ui = assert(loadScript(SCRIPT_HOME.."/ui.lua"))()
+
+return { run=run_ui }

SCRIPTS/EMU/IMUF.lua

@@ -0,0 +1,26 @@
+
+return {
+    read           = 227, -- MSP_IMUF_CONFIG
+    write          = 228, -- MSP_SET_IMUF_CONFIG
+    title          = "IMU-F.",
+    reboot         = false,
+    eepromWrite    = true,
+    yMinLimit      = 11,
+    yMaxLimit      = 52,
+    minBytes          = 9,
+    text = {
+        { t = "IMU-F Q",      x = 2,  y = 12,  to = SMLSIZE },
+        { t = "Roll Q",       x = 12, y = 20,  to = SMLSIZE },
+        { t = "Pitch Q",       x = 12, y = 28,  to = SMLSIZE },
+        { t = "Yaw Q",       x = 12, y = 36,  to = SMLSIZE },
+        { t = "IMU-F W",       x = 2, y = 44,  to = SMLSIZE },
+        { t = "IMU-F SHARPNESS",       x = 2, y = 52,  to = SMLSIZE },
+    },
+    fields = {
+        { x = 87, y = 20,  to=SMLSIZE, min = 0,   max = 16000,    vals = { 3, 4 } },
+        { x = 87, y = 28,  to=SMLSIZE, min = 0,    max = 160000,   vals = { 5, 6 } },
+        { x = 87, y = 36,  to=SMLSIZE, min = 0,    max = 160000,    vals = { 7, 8 } },
+        { x = 87, y = 44,  to=SMLSIZE, min = 0,    max = 256,     vals = { 9, 10 } },
+        { x = 87, y = 52,  to=SMLSIZE, min = 0,    max = 160000,     vals = { 11, 12 } },
+    },
+}

SCRIPTS/EMU/MSP/common.lua

@@ -0,0 +1,139 @@
+
+-- Protocol version
+MSP_VERSION = bit32.lshift(1,5)
+MSP_STARTFLAG = bit32.lshift(1,4)
+
+-- Sequence number for next MSP packet
+local mspSeq = 0
+local mspRemoteSeq = 0
+local mspRxBuf = {}
+local mspRxIdx = 1
+local mspRxCRC = 0
+local mspRxReq = 0
+local mspStarted = false
+local mspLastReq = 0
+local mspTxBuf = {}
+local mspTxIdx = 1
+local mspTxCRC = 0
+local mspTxPk = 0
+
+mspPendingRequest = false
+
+function mspProcessTxQ()
+    if (#(mspTxBuf) == 0) then
+        return false
+    end
+    if not protocol.push() then
+        return true
+    end
+    local payload = {}
+    payload[1] = mspSeq + MSP_VERSION
+    mspSeq = bit32.band(mspSeq + 1, 0x0F)
+    if mspTxIdx == 1 then
+        -- start flag
+        payload[1] = payload[1] + MSP_STARTFLAG
+    end
+    local i = 2
+    while (i <= protocol.maxTxBufferSize) do
+        if mspTxIdx > #(mspTxBuf) then
+            break
+        end
+        payload[i] = mspTxBuf[mspTxIdx]
+        mspTxIdx = mspTxIdx + 1
+        mspTxCRC = bit32.bxor(mspTxCRC,payload[i])  
+        i = i + 1
+    end
+    if i <= protocol.maxTxBufferSize then
+        payload[i] = mspTxCRC
+        i = i + 1
+      -- zero fill
+        while i <= protocol.maxTxBufferSize do
+            payload[i] = 0
+            i = i + 1
+        end
+        if protocol.mspSend(payload) then
+            mspTxPk = mspTxPk + 1
+        end
+        mspTxBuf = {}
+        mspTxIdx = 1
+        mspTxCRC = 0     
+        return false
+    end
+    if protocol.mspSend(payload) then
+        mspTxPk = mspTxPk + 1
+    end
+    return true
+end
+
+function mspSendRequest(cmd, payload)
+    -- busy
+    if #(mspTxBuf) ~= 0 or not cmd then
+        return nil
+    end
+    mspTxBuf[1] = #(payload)
+    mspTxBuf[2] = bit32.band(cmd,0xFF)  -- MSP command
+    for i=1,#(payload) do
+        mspTxBuf[i+2] = bit32.band(payload[i],0xFF)
+    end
+    mspLastReq = cmd
+    return mspProcessTxQ()
+end
+
+function mspReceivedReply(payload)
+    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
+        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)
+        mspRxReq  = mspLastReq
+        idx = idx + 1
+        mspStarted = true
+    elseif not mspStarted then
+        return nil
+    elseif bit32.band(mspRemoteSeq + 1, 0x0F) ~= seq then
+        mspStarted = false
+        return nil
+    end
+    while (idx <= protocol.maxRxBufferSize) and (mspRxIdx <= mspRxSize) do
+        mspRxBuf[mspRxIdx] = payload[idx]
+        mspRxCRC = bit32.bxor(mspRxCRC,payload[idx])
+        mspRxIdx = mspRxIdx + 1
+        idx = idx + 1
+    end
+    if idx > protocol.maxRxBufferSize then
+        mspRemoteSeq = seq
+        return true
+    end
+    -- check CRC
+    if mspRxCRC ~= payload[idx] then
+        mspStarted = false
+        return nil
+    end
+    mspStarted = false
+    return mspRxBuf
+end
+
+function mspPollReply()
+    while true do
+        ret = protocol.mspPoll()
+        if type(ret) == "table" then
+            mspLastReq = 0
+            return mspRxReq, ret
+        else
+            break
+        end
+    end
+    return nil
+end

SCRIPTS/EMU/MSP/crsf.lua

@@ -0,0 +1,43 @@
+
+-- CRSF Devices
+CRSF_ADDRESS_BETAFLIGHT          = 0xC8
+CRSF_ADDRESS_RADIO_TRANSMITTER   = 0xEA
+-- CRSF Frame Types
+CRSF_FRAMETYPE_MSP_REQ           = 0x7A      -- response request using msp sequence as command
+CRSF_FRAMETYPE_MSP_RESP          = 0x7B      -- reply with 60 byte chunked binary
+CRSF_FRAMETYPE_MSP_WRITE         = 0x7C      -- write with 60 byte chunked binary 
+
+crsfMspCmd = 0
+crsfMspHeader = {}
+
+protocol.mspSend = function(payload)
+    local payloadOut = { CRSF_ADDRESS_BETAFLIGHT, CRSF_ADDRESS_RADIO_TRANSMITTER }
+    for i=1, #(payload) do
+        payloadOut[i+2] = payload[i]
+    end
+    return crossfireTelemetryPush(crsfMspCmd, payloadOut)
+end
+
+protocol.mspRead = function(cmd)
+    crsfMspCmd = CRSF_FRAMETYPE_MSP_REQ
+    return mspSendRequest(cmd, {})
+end
+
+protocol.mspWrite = function(cmd, payload)
+    crsfMspCmd = CRSF_FRAMETYPE_MSP_WRITE
+    return mspSendRequest(cmd, payload)
+end
+
+protocol.mspPoll = function()
+    local command, data = crossfireTelemetryPop()
+    if command == CRSF_FRAMETYPE_MSP_RESP then
+        if data[1] == CRSF_ADDRESS_RADIO_TRANSMITTER and data[2] == CRSF_ADDRESS_BETAFLIGHT then
+            local mspData = {}
+            for i=3, #(data) do
+                mspData[i-2] = data[i]
+            end
+            return mspReceivedReply(mspData)
+        end
+    end
+    return nil
+end

SCRIPTS/EMU/MSP/messages.lua

@@ -0,0 +1,85 @@
+MSP_PID_FORMAT = {
+   read           = 112, -- MSP_PID
+   write          = 202, -- MSP_SET_PID
+   minBytes       = 8,
+   fields = {
+      -- P
+      { vals = { 1 } },
+      { vals = { 4 } },
+      { vals = { 7 } },
+      -- I
+      { vals = { 2 } },
+      { vals = { 5 } },
+      { vals = { 8 } },
+      -- D
+      { vals = { 3 } },
+      { vals = { 6 } },
+   },
+}
+
+MSP_PID_ADVANCED_FORMAT = {
+   read           = 94, -- MSP_PID_ADVANCED
+   write          = 95, -- MSP_SET_PID_ADVANCED
+   minBytes       = 23,
+   fields = {
+	  -- weight
+      { vals = { 10 }, scale = 100 },
+	  -- transition
+      { vals = { 9 }, scale = 100 },
+   },
+}
+
+local INTRO_DELAY = 1600
+local READOUT_DELAY = 500
+
+function extractMspValues(cmd, rx_buf, msgFormat, msgValues)
+    if cmd == nil or rx_buf == nil then
+        return
+    end
+    if cmd ~= msgFormat.read then
+        return
+    end
+    if #(rx_buf) > 0 then
+        msgValues.raw = {}
+        for i=1,#(rx_buf) do
+            msgValues.raw[i] = rx_buf[i]
+        end
+
+		msgValues.values = {}
+        for i=1,#(msgFormat.fields) do
+            if (#(msgValues.raw) or 0) >= msgFormat.minBytes then
+               local f = msgFormat.fields[i]
+               if f.vals then
+                  local value = 0;
+                  for idx=1, #(f.vals) do
+                     local raw_val = msgValues.raw[f.vals[idx]]
+                     raw_val = bit32.lshift(raw_val, (idx-1)*8)
+                     value = bit32.bor(value, raw_val)
+                  end
+                  msgValues.values[i] = value/(f.scale or 1)
+               end
+            end
+        end
+    end
+end
+
+function readoutMsp(msgFormat, msg)
+    local t = getTime()
+    if msg.lastTrigger == nil or msg.lastTrigger + INTRO_DELAY <= t then
+        playFile(msg.intro)
+        msg.lastTrigger = t
+    elseif msg.reqTS == nil or msg.reqTS + READOUT_DELAY <= t then
+        protocol.mspRead(msgFormat.read)
+        msg.reqTS = t
+    else
+        local cmd, rx_buf = mspPollReply()
+        extractMspValues(cmd, rx_buf, msgFormat, msg)
+        if msg.raw then
+            for i=1,#(msg.readoutValues) do
+                playNumber(msg.values[msg.readoutValues[i]], 0)
+            end
+            msg.raw = nil
+        end
+    end
+    mspProcessTxQ()
+end

SCRIPTS/EMU/MSP/sp.lua

@@ -0,0 +1,63 @@
+
+LOCAL_SENSOR_ID  = 0x0D
+SMARTPORT_REMOTE_SENSOR_ID = 0x1B
+FPORT_REMOTE_SENSOR_ID = 0x00
+REQUEST_FRAME_ID = 0x30
+REPLY_FRAME_ID   = 0x32
+
+local lastSensorId, lastFrameId, lastDataId, lastValue
+
+protocol.mspSend = function(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)
+    return sportTelemetryPush(LOCAL_SENSOR_ID, REQUEST_FRAME_ID, dataId, value)
+end
+
+protocol.mspRead = function(cmd)
+    return mspSendRequest(cmd, {})
+end
+
+protocol.mspWrite = function(cmd, payload)
+    return mspSendRequest(cmd, payload)
+end
+
+--Discards duplicate data from lua input buffer
+local function smartPortTelemetryPop()
+    local sensorId, frameId, dataId, value
+    while true do
+        sensorId, frameId, dataId, value = sportTelemetryPop()
+        if sensorId == nil then
+            return nil
+        elseif (lastSensorId == sensorId) and (lastFrameId == frameId) and (lastDataId == dataId) and (lastValue == value) then
+            --Keep checking
+        else
+            lastSensorId = sensorId
+            lastFrameId = frameId
+            lastDataId = dataId
+            lastValue = value
+            return sensorId, frameId, dataId, value
+        end
+    end
+end
+
+protocol.mspPoll = function()
+    local sensorId, frameId, dataId, value = smartPortTelemetryPop()
+    if (sensorId == SMARTPORT_REMOTE_SENSOR_ID or sensorId == FPORT_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)
+        return mspReceivedReply(payload)
+    end
+    return nil
+end