S6R support (#3896)

radio/sdcard/horus/S6R/S6R.lua

@@ -0,0 +1,330 @@
+local VALUE = 0
+local COMBO = 1
+
+local COLUMN_2 = 300
+
+local edit = false
+local page = 1
+local current = 1
+local refreshState = 0
+local refreshIndex = 0
+local calibrationState = 0
+local pageOffset = 0
+local calibrationStep = 0
+local pages = {}
+local fields = {}
+local modifications = {}
+
+local configFields = {
+  {"Wing type:", COMBO, 0x80, nil, { "Normal", "Delta", "VTail" } },
+  {"Mounting type:", COMBO, 0x81, nil, { "Horz", "Horz rev.", "Vert", "Vert rev." } },
+}
+
+local settingsFields = {
+  {"S6R functions:", COMBO, 0x9C, nil, { "Disable", "Enable" } },
+  {"AIL direction:", COMBO, 0x82, nil, { "Normal", "Invers" }, { 255, 0 } },
+  {"ELE direction:", COMBO, 0x83, nil, { "Normal", "Invers" }, { 255, 0 } },
+  {"RUD direction:", COMBO, 0x84, nil, { "Normal", "Invers" }, { 255, 0 } },
+  {"AIL2 direction:", COMBO, 0x9A, nil, { "Normal", "Invers" }, { 255, 0 } },
+  {"ELE2 direction:", COMBO, 0x9B, nil, { "Normal", "Invers" }, { 255, 0 } },
+  {"AIL stabilize gain:", VALUE, 0x85, nil, 0, 200, "%"},
+  {"ELE stabilize gain:", VALUE, 0x86, nil, 0, 200, "%"},
+  {"RUD stabilize gain:", VALUE, 0x87, nil, 0, 200, "%"},
+  {"AIL auto level gain:", VALUE, 0x88, nil, 0, 200, "%"},
+  {"ELE auto level gain:", VALUE, 0x89, nil, 0, 200, "%"},
+  {"ELE upright gain:", VALUE, 0x8C, nil, 0, 200, "%"},
+  {"RUD upright gain:", VALUE, 0x8D, nil, 0, 200, "%"},
+  {"AIL crab gain:", VALUE, 0x8E, nil, 0, 200, "%"},
+  {"RUD crab gain:", VALUE, 0x90, nil, 0, 200, "%"},
+  {"AIL auto angle offset:", VALUE, 0x91, nil, -20, 20, "%", 0x6C},
+  {"ELE auto angle offset:", VALUE, 0x92, nil, -20, 20, "%", 0x6C},
+  {"ELE upright angle offset:", VALUE, 0x95, nil, -20, 20, "%", 0x6C},
+  {"RUD upright angle offset:", VALUE, 0x96, nil, -20, 20, "%", 0x6C},
+  {"AIL crab angle offset:", VALUE, 0x97, nil, -20, 20, "%", 0x6C},
+  {"RUD crab angle offset:", VALUE, 0x99, nil, -20, 20, "%", 0x6C},
+}
+
+local calibrationFields = {
+  {"X:", VALUE, 0x9E, 0, -100, 100, "%"},
+  {"Y:", VALUE, 0x9F, 0, -100, 100, "%"},
+  {"Z:", VALUE, 0xA0, 0, -100, 100, "%"}
+}
+
+local function drawScreenTitle(title,page, pages)
+	lcd.drawFilledRectangle(0, 0, LCD_W, 30, TITLE_BGCOLOR)
+	lcd.drawText(1, 5, title, MENU_TITLE_COLOR)
+	lcd.drawText(LCD_W-40, 5, page.."/"..pages, MENU_TITLE_COLOR)
+end
+
+-- Change display attribute to current field
+local function addField(step)
+  local field = fields[current]
+  local min, max
+  if field[2] == VALUE then
+    min = field[5]
+    max = field[6]
+  elseif field[2] == COMBO then
+    min = 0
+    max = #(field[5]) - 1
+  end
+  if (step < 0 and field[4] > min) or (step > 0 and field[4] < max) then
+    field[4] = field[4] + step
+  end
+end
+
+-- Select the next or previous page
+local function selectPage(step)
+  page = 1 + ((page + step - 1 + #pages) % #pages)
+  refreshIndex = 0
+  calibrationStep = 0
+  pageOffset = 0
+end
+
+-- Select the next or previous editable field
+local function selectField(step)
+  current = 1 + ((current + step - 1 + #fields) % #fields)
+  if current > 7 + pageOffset then
+    pageOffset = current - 7
+  elseif current <= pageOffset then
+    pageOffset = current - 1
+  end
+end
+
+local function drawProgressBar()
+  local width = (300 * refreshIndex) / #fields
+  lcd.drawRectangle(100, 10, 300, 6)
+  lcd.drawFilledRectangle(102, 13, width, 2);
+end
+
+-- Redraw the current page
+local function redrawFieldsPage(event)
+  lcd.clear()
+  lcd.drawFilledRectangle(0, 0, LCD_W, LCD_H, TEXT_BGCOLOR)
+  drawScreenTitle("S6R", page, #pages)
+
+  if refreshIndex < #fields then
+    drawProgressBar()
+  end
+
+  for index = 1, 10, 1 do
+    local field = fields[pageOffset+index]
+    if field == nil then
+      break
+    end
+
+    local attr = current == (pageOffset+index) and ((edit == true and BLINK or 0) + INVERS) or 0
+	attr = attr + TEXT_COLOR
+
+    lcd.drawText(1, 30+20*index, field[1], TEXT_COLOR)
+
+    if field[4] == nil then
+      lcd.drawText(COLUMN_2, 30+20*index, "---", attr)
+    else
+      if field[2] == VALUE then
+        lcd.drawNumber(COLUMN_2, 30+20*index, field[4], LEFT + attr)
+      elseif field[2] == COMBO then
+        if field[4] >= 0 and field[4] < #(field[5]) then
+          lcd.drawText(COLUMN_2, 30+20*index, field[5][1+field[4]], attr)
+        end
+      end
+    end
+  end
+end
+
+local function telemetryRead(field)
+  return sportTelemetryPush(0x1B, 0x30, 0x0C30, field)
+end
+
+local function telemetryWrite(field, value)
+  return sportTelemetryPush(0x1B, 0x31, 0x0C30, field + value*256)
+end
+
+local telemetryPopTimeout = 0
+local function refreshNext()
+  if refreshState == 0 then
+    if calibrationState == 1 then
+      if telemetryWrite(0x9D, calibrationStep) == true then
+        refreshState = 1
+        calibrationState = 2
+        telemetryPopTimeout = getTime() + 80 -- normal delay is 500ms
+      end
+    elseif #modifications > 0 then
+      telemetryWrite(modifications[1][1], modifications[1][2])
+      modifications[1] = nil
+    elseif refreshIndex < #fields then
+      local field = fields[refreshIndex + 1]
+      if telemetryRead(field[3]) == true then
+        refreshState = 1
+        telemetryPopTimeout = getTime() + 80 -- normal delay is 500ms
+      end
+    end
+  elseif refreshState == 1 then
+    local physicalId, primId, dataId, value = sportTelemetryPop()
+    if physicalId == 0x1A and primId == 0x32 and dataId == 0x0C30 then
+      local fieldId = value % 256
+      if calibrationState == 2 then
+        if fieldId == 0x9D then
+          refreshState = 0
+          calibrationState = 0
+          calibrationStep = (calibrationStep + 1) % 7
+        end
+      else
+        local field = fields[refreshIndex + 1]
+        if fieldId == field[3] then
+          local value = math.floor(value / 256)
+          if field[3] >= 0x9E and field[3] <= 0xA0 then
+            local b1 = value % 256
+            local b2 = math.floor(value / 256)
+            value = b1*256 + b2
+            value = value - bit32.band(value, 0x8000) * 2
+          end
+          if field[2] == COMBO and #field == 6 then
+            for index = 1, #(field[6]), 1 do
+              if value == field[6][index] then
+                value = index - 1
+                break
+              end
+            end
+          elseif field[2] == VALUE and #field == 8 then
+            value = value - field[8] + field[5]
+          end
+          fields[refreshIndex + 1][4] = value
+          refreshIndex = refreshIndex + 1
+          refreshState = 0
+        end
+      end
+    elseif getTime() > telemetryPopTimeout then
+      refreshState = 0
+      calibrationState = 0
+    end
+  end
+end
+
+local function updateField(field)
+  local value = field[4]
+  if field[2] == COMBO and #field == 6 then
+    value = field[6][1+value]
+  elseif field[2] == VALUE and #field == 8 then
+    value = value + field[8] - field[5]
+  end
+  modifications[#modifications+1] = { field[3], value }
+end
+
+-- Main
+local function runFieldsPage(event)
+  if event == EVT_EXIT_BREAK  then             -- exit script
+    return 2
+  elseif event == EVT_ENTER_BREAK or event == EVT_ROT_BREAK then        -- toggle editing/selecting current field
+    if fields[current][4] ~= nil then
+      edit = not edit
+      if edit == false then
+        updateField(fields[current])
+      end
+    end
+  elseif edit then
+    if event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT or event == EVT_PLUS_REPT then
+      addField(1)
+    elseif event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then
+      addField(-1)
+    end
+  else
+    if event == EVT_MINUS_FIRST or event == EVT_ROT_RIGHT then
+      selectField(1)
+    elseif event == EVT_PLUS_FIRST or event == EVT_ROT_LEFT then
+      selectField(-1)
+    end
+  end
+  redrawFieldsPage(event)
+  return 0
+end
+
+local wingBitmaps = { Bitmap.open("img/plane_b.png"), Bitmap.open("img/delta_b.png"), Bitmap.open("img/planev_b.png") }
+local mountBitmaps = { Bitmap.open("img/up.png"), Bitmap.open("img/down.png"), Bitmap.open("img/vert.png"), Bitmap.open("img/vert-r.png") }
+
+local function runConfigPage(event)
+  fields = configFields
+  local result = runFieldsPage(event)
+  if fields[1][4] ~= nil then
+    lcd.drawBitmap(wingBitmaps[1 + fields[1][4]], 10, 90)
+  end
+  if fields[2][4] ~= nil then
+    lcd.drawBitmap(mountBitmaps[1 + fields[2][4]], 190, 110)
+  end
+  return result
+end
+
+local function runSettingsPage(event)
+  fields = settingsFields
+  return runFieldsPage(event)
+end
+
+local calibrationPositions = { "up", "down", "left", "right", "forward", "back" }
+
+
+local function runCalibrationPage(event)
+  fields = calibrationFields
+  if refreshIndex == #fields then
+    refreshIndex = 0
+  end
+  lcd.clear()
+    lcd.drawFilledRectangle(0, 0, LCD_W, LCD_H, TEXT_BGCOLOR)
+  drawScreenTitle("S6R", page, #pages)
+  if(calibrationStep < 6) then
+    local position = calibrationPositions[1 + calibrationStep]
+    lcd.drawText(200, 50, "Turn the S6R " .. position, TEXT_COLOR)
+    lcd.drawBitmap(Bitmap.open("img/"..position .. ".png"), 200, 70)
+    for index = 1, 3, 1 do
+      local field = fields[index]
+      lcd.drawText(80, 50+20*index, field[1]..":", TEXT_COLOR)
+      lcd.drawNumber(100, 50+20*index, field[4]/10, LEFT+PREC2)
+    end
+
+    local attr = calibrationState == 0 and INVERS or 0
+    lcd.drawText(160, 220, "Press [Enter] when ready", attr)
+  else
+    lcd.drawText(160, 50, "Calibration completed", 0)
+    lcd.drawBitmap(Bitmap.open("img/done.bmp"),200, 100)
+    lcd.drawText(160, 220, "Press [RTN] when ready", attr)
+  end
+  if calibrationStep > 6 and (event == EVT_ENTER_BREAK or event == EVT_EXIT_BREAK) then
+    return 2
+  elseif event == EVT_ENTER_BREAK then
+    calibrationState = 1
+  elseif event == EVT_EXIT_BREAK then
+    if calibrationStep > 0 then
+      calibrationStep = 0
+    end
+  end
+  return 0
+
+end
+
+-- Init
+local function init()
+  current, edit, refreshState, refreshIndex = 1, false, 0, 0
+  pages = {
+    runConfigPage,
+    runSettingsPage,
+    runCalibrationPage
+  }
+end
+
+-- Main
+local function run(event)
+  if event == nil then
+    error("Cannot be run as a model script!")
+    return 2
+  elseif event == EVT_PAGE_BREAK or event == EVT_PAGEDN_FIRST  then
+    selectPage(1)
+  elseif event == EVT_PAGE_LONG or event == EVT_PAGEUP_FIRST then
+    killEvents(event);
+    selectPage(-1)
+  end
+
+  local result = pages[page](event)
+  refreshNext()
+
+  return result
+end
+
+return { init=init, run=run }

radio/sdcard/taranis/S6R/S6R.lua

@@ -196,9 +196,9 @@ local function runFieldsPage(event)
       addField(-1)
     end
   else
-    if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT then
+    if event == EVT_MINUS_FIRST or event == EVT_ROT_RIGHT then
       selectField(1)
-    elseif event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT then
+    elseif event == EVT_PLUS_FIRST or event == EVT_ROT_LEFT then
       selectField(-1)
     end
   end