Merge pull request #1522 from raphaelcoeffic/sport_msp

MSP/SPORT: added a new PID configuration script (+ minor fix)

src/test/BFSetup.lua

@@ -0,0 +1,599 @@
+--
+-- 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 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 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,
+   }
+}
+
+local currentPage = 1
+local currentLine = 1
+
+local function saveSettings()
+   local page = SetupPages[currentPage]
+   if page.values then
+      mspSendRequest(page.write,page.values)
+      gState = PAGE_SAVING
+   end
+end
+
+local function invalidatePages()
+   for i=1,#(SetupPages) do
+      local page = SetupPages[i]
+      page.values = nil
+   end
+end
+
+local menuList = {
+
+   { t = "save page",
+     f = saveSettings },
+
+   { t = "reload",
+     f = invalidatePages }
+}
+
+local telemetryScreenActive = false
+local menuActive = false
+--local editingValue = 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
+      gState = PAGE_DISPLAY
+      page.values = nil
+      return
+   end
+   
+   if cmd ~= page.read then
+      return
+   end
+
+   page.values = {}
+   for i=1,#(rx_buf) do
+      page.values[i] = rx_buf[i]
+   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
+
+   if page_locked then
+      screen_title = screen_title .. " (...)"
+   end
+   
+   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
+         lcd.drawText(f.x + spacing, f.y, page.values[idx], text_options)
+      else
+         lcd.drawText(f.x + spacing, f.y, "---", text_options)
+      end
+   end
+end
+
+local function clipValue(val)
+   if val < 0 then
+      val = 0
+   elseif val > 255 then
+      val = 255
+   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)
+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 bgCounter = 0
+
+local function run(event)
+
+   local now = getTime()
+
+   if not telemetryScreenActive then
+      telemetryScreenActive = true
+      invalidatePages()
+   end
+
+   if #(mspTxBuf) > 0 then
+      mspProcessTxQ()
+      --if (gState == PAGE_SAVING) and (#(mspTxBuf) == 0) then
+      --   gState = PAGE_REFRESH
+      --end
+   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 then
+         incMenu(-1)
+      elseif event == EVT_MINUS_BREAK 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 then
+         incLine(-1)
+      elseif event == EVT_MINUS_BREAK 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] 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 then
+         incValue(1)
+      elseif event == EVT_MINUS_FIRST or event == EVT_MINUS_REPT 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)
+   end
+
+   --lcd.drawNumber(10,56,mspRequestsSent)
+   --lcd.drawNumber(30,56,mspTxPk)
+   
+   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
+
+local function background()
+--   telemetryScreenActive = false
+--   bgCounter = bgCounter + 1
+end
+
+return {run=run,background=background}

src/test/SpMsp.lua

@@ -134,6 +134,7 @@ local function mspReceivedReply(payload)
 end
 
 local function mspPollReply()
+   while true do
       local sensorId, frameId, dataId, value = sportTelemetryPop()
       if sensorId == REMOTE_SENSOR_ID and frameId == REPLY_FRAME_ID then
 
@@ -151,6 +152,9 @@ local function mspPollReply()
          payload[6] = bit32.band(value,0xFF)
 
          return mspReceivedReply(payload)
+      else
+         break
+      end
    end
 end