radio/sdcard/taranis-x9/SxR/SxR_Calibrate.lua

   1 ---- #########################################################################
   2 ---- #                                                                       #
   3 ---- # Copyright (C) OpenTX                                                  #
   4 -----#                                                                       #
   5 ---- # License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html               #
   6 ---- #                                                                       #
   7 ---- # This program is free software; you can redistribute it and/or modify  #
   8 ---- # it under the terms of the GNU General Public License version 2 as     #
   9 ---- # published by the Free Software Foundation.                            #
  10 ---- #                                                                       #
  11 ---- # This program is distributed in the hope that it will be useful        #
  12 ---- # but WITHOUT ANY WARRANTY; without even the implied warranty of        #
  13 ---- # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         #
  14 ---- # GNU General Public License for more details.                          #
  15 ---- #                                                                       #
  16 ---- #########################################################################
  17 local VALUE = 0
  18 local COMBO = 1
  19
  20 local COLUMN_2 = 150
  21
  22 local edit = false
  23 local page = 1
  24 local current = 1
  25 local refreshState = 0
  26 local refreshIndex = 0
  27 local calibrationState = 0
  28 local pageOffset = 0
  29 local calibrationStep = 0
  30 local pages = {}
  31 local fields = {}
  32 local modifications = {}
  33
  34 local calibrationFields = {
  35   {"X:", VALUE, 0x9E, 0, -100, 100, "%"},
  36   {"Y:", VALUE, 0x9F, 0, -100, 100, "%"},
  37   {"Z:", VALUE, 0xA0, 0, -100, 100, "%"}
  38 }
  39
  40 local function drawProgressBar()
  41   local width = (140 * refreshIndex) / #fields
  42   lcd.drawRectangle(30, 1, 144, 6)
  43   lcd.drawFilledRectangle(32, 3, width, 2);
  44 end
  45
  46 -- Select the next or previous page
  47 local function selectPage(step)
  48   page = 1 + ((page + step - 1 + #pages) % #pages)
  49   refreshIndex = 0
  50   calibrationStep = 0
  51   pageOffset = 0
  52 end
  53
  54 -- Draw initial warning page
  55 local function runWarningPage(event)
  56   lcd.clear()
  57   lcd.drawScreenTitle("S6R", page, #pages)
  58   lcd.drawText(0, 10, "Warning: this will start S6R calibration", SMLSIZE)
  59   lcd.drawText(0, 20, "This need to be run only once. You need a S6R,", SMLSIZE)
  60   lcd.drawText(0, 30, "power supply and a flat level surface (desk,...)", SMLSIZE)
  61   lcd.drawText(0, 40, "Press [Enter] when ready", SMLSIZE)
  62   lcd.drawText(0, 50, "Press [Exit] when cancel", SMLSIZE)
  63   if event == EVT_ENTER_BREAK then
  64     selectPage(1)
  65     return 0
  66   elseif event == EVT_EXIT_BREAK then
  67     return 2
  68   end
  69   return 0
  70 end
  71
  72 -- Redraw the current page
  73 local function redrawFieldsPage()
  74   lcd.clear()
  75   lcd.drawScreenTitle("S6R", page, #pages)
  76
  77   if refreshIndex < #fields then
  78     drawProgressBar()
  79   end
  80
  81   for index = 1, 7, 1 do
  82     local field = fields[pageOffset+index]
  83     if field == nil then
  84       break
  85     end
  86
  87     local attr = current == (pageOffset+index) and ((edit == true and BLINK or 0) + INVERS) or 0
  88
  89     lcd.drawText(0, 1+8*index, field[1])
  90
  91     if field[4] == nil then
  92       lcd.drawText(COLUMN_2, 1+8*index, "---", attr)
  93     else
  94       if field[2] == VALUE then
  95         lcd.drawNumber(COLUMN_2, 1+8*index, field[4], LEFT + attr)
  96       elseif field[2] == COMBO then
  97         if field[4] >= 0 and field[4] < #(field[5]) then
  98           lcd.drawText(COLUMN_2, 1+8*index, field[5][1+field[4]], attr)
  99         end
 100       end
 101     end
 102   end
 103 end
 104
 105 local function telemetryRead(field)
 106   return sportTelemetryPush(0x17, 0x30, 0x0C30, field)
 107 end
 108
 109 local function telemetryWrite(field, value)
 110   return sportTelemetryPush(0x17, 0x31, 0x0C30, field + value*256)
 111 end
 112
 113 local telemetryPopTimeout = 0
 114 local function refreshNext()
 115   if refreshState == 0 then
 116     if calibrationState == 1 then
 117       if telemetryWrite(0x9D, calibrationStep) == true then
 118         refreshState = 1
 119         calibrationState = 2
 120         telemetryPopTimeout = getTime() + 80 -- normal delay is 500ms
 121       end
 122     elseif #modifications > 0 then
 123       telemetryWrite(modifications[1][1], modifications[1][2])
 124       modifications[1] = nil
 125     elseif refreshIndex < #fields then
 126       local field = fields[refreshIndex + 1]
 127       if telemetryRead(field[3]) == true then
 128         refreshState = 1
 129         telemetryPopTimeout = getTime() + 80 -- normal delay is 500ms
 130       end
 131     end
 132   elseif refreshState == 1 then
 133     local physicalId, primId, dataId, value = sportTelemetryPop()
 134     if physicalId == 0x1A and primId == 0x32 and dataId == 0x0C30 then
 135       local fieldId = value % 256
 136       if calibrationState == 2 then
 137         if fieldId == 0x9D then
 138           refreshState = 0
 139           calibrationState = 0
 140           calibrationStep = (calibrationStep + 1) % 7
 141         end
 142       else
 143         local field = fields[refreshIndex + 1]
 144         if fieldId == field[3] then
 145           local value = math.floor(value / 256)
 146           if field[3] >= 0x9E and field[3] <= 0xA0 then
 147             local b1 = value % 256
 148             local b2 = math.floor(value / 256)
 149             value = b1*256 + b2
 150             value = value - bit32.band(value, 0x8000) * 2
 151           end
 152           if field[2] == COMBO and #field == 6 then
 153             for index = 1, #(field[6]), 1 do
 154               if value == field[6][index] then
 155                 value = index - 1
 156                 break
 157               end
 158             end
 159           elseif field[2] == VALUE and #field == 8 then
 160             value = value - field[8] + field[5]
 161           end
 162           fields[refreshIndex + 1][4] = value
 163           refreshIndex = refreshIndex + 1
 164           refreshState = 0
 165         end
 166       end
 167     elseif getTime() > telemetryPopTimeout then
 168       refreshState = 0
 169       calibrationState = 0
 170     end
 171   end
 172 end
 173
 174 local function updateField(field)
 175   local value = field[4]
 176   if field[2] == COMBO and #field == 6 then
 177     value = field[6][1+value]
 178   elseif field[2] == VALUE and #field == 8 then
 179     value = value + field[8] - field[5]
 180   end
 181   modifications[#modifications+1] = { field[3], value }
 182 end
 183
 184 -- Main
 185 local function runFieldsPage(event)
 186   if event == EVT_EXIT_BREAK then -- exit script
 187     return 2
 188   elseif event == EVT_ENTER_BREAK then -- toggle editing/selecting current field
 189     if fields[current][4] ~= nil then
 190       edit = not edit
 191       if edit == false then
 192         updateField(fields[current])
 193       end
 194     end
 195   elseif edit then
 196     if event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT or event == EVT_PLUS_REPT then
 197       addField(1)
 198     elseif event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then
 199       addField(-1)
 200     end
 201   else
 202     if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT then
 203       selectField(1)
 204     elseif event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT then
 205       selectField(-1)
 206     end
 207   end
 208   redrawFieldsPage()
 209   return 0
 210 end
 211
 212 local calibrationPositionsBitmaps = { "bmp/up.bmp", "bmp/down.bmp", "bmp/left.bmp", "bmp/right.bmp", "bmp/forward.bmp", "bmp/back.bmp"  }
 213
 214 local function runCalibrationPage(event)
 215   fields = calibrationFields
 216   if refreshIndex == #fields then
 217     refreshIndex = 0
 218   end
 219   lcd.clear()
 220   lcd.drawScreenTitle("S6R", page, #pages)
 221   if(calibrationStep < 6) then
 222     lcd.drawText(0, 9, "Turn the S6R as shown", 0)
 223     lcd.drawPixmap(10, 19, calibrationPositionsBitmaps[1 + calibrationStep])
 224     for index = 1, 3, 1 do
 225       local field = fields[index]
 226       lcd.drawText(80, 12+10*index, field[1], 0)
 227       lcd.drawNumber(90, 12+10*index, field[4]/10, LEFT+PREC2)
 228     end
 229
 230     local attr = calibrationState == 0 and INVERS or 0
 231     lcd.drawText(0, 56, "Press [Enter] when ready", attr)
 232   else
 233     lcd.drawText(0, 9, "Calibration completed", 0)
 234     lcd.drawPixmap(10, 19, "bmp/done.bmp")
 235     lcd.drawText(0, 56, "Press [Exit] when ready", attr)
 236   end
 237   if calibrationStep > 6 and (event == EVT_ENTER_BREAK or event == EVT_EXIT_BREAK) then
 238     return 2
 239   elseif event == EVT_ENTER_BREAK then
 240     calibrationState = 1
 241   elseif event == EVT_EXIT_BREAK then
 242     if calibrationStep > 0 then
 243       calibrationStep = 0
 244     end
 245   end
 246   return 0
 247 end
 248
 249 -- Init
 250 local function init()
 251   current, edit, refreshState, refreshIndex = 1, false, 0, 0
 252   pages = {
 253     runWarningPage,
 254     runCalibrationPage
 255   }
 256 end
 257
 258 -- Main
 259 local function run(event)
 260   if event == nil then
 261     error("Cannot be run as a model script!")
 262     return 2
 263   elseif event == EVT_PAGE_BREAK then
 264     selectPage(1)
 265   elseif event == EVT_PAGE_LONG then
 266     killEvents(event);
 267     selectPage(-1)
 268   end
 269
 270   local result = pages[page](event)
 271   refreshNext()
 272
 273   return result
 274 end
 275
 276 return { init=init, run=run }