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("SxR", page, #pages)
  58   lcd.drawText(0, 10, "Warning: this will start SxR calibration", SMLSIZE)
  59   lcd.drawText(0, 20, "This need to be run only once. You need a SxR,", 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("SxR", 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           value =  bit32.band(value, 0xffff)
 147           if field[3] >= 0x9E and field[3] <= 0xA0 then
 148             local b1 = value % 256
 149             local b2 = math.floor(value / 256)
 150             value = b1*256 + b2
 151             value = value - bit32.band(value, 0x8000) * 2
 152           end
 153           if field[2] == COMBO and #field == 6 then
 154             for index = 1, #(field[6]), 1 do
 155               if value == field[6][index] then
 156                 value = index - 1
 157                 break
 158               end
 159             end
 160           elseif field[2] == VALUE and #field == 8 then
 161             value = value - field[8] + field[5]
 162           end
 163           fields[refreshIndex + 1][4] = value
 164           refreshIndex = refreshIndex + 1
 165           refreshState = 0
 166         end
 167       end
 168     elseif getTime() > telemetryPopTimeout then
 169       refreshState = 0
 170       calibrationState = 0
 171     end
 172   end
 173 end
 174
 175 local function updateField(field)
 176   local value = field[4]
 177   if field[2] == COMBO and #field == 6 then
 178     value = field[6][1+value]
 179   elseif field[2] == VALUE and #field == 8 then
 180     value = value + field[8] - field[5]
 181   end
 182   modifications[#modifications+1] = { field[3], value }
 183 end
 184
 185 -- Main
 186 local function runFieldsPage(event)
 187   if event == EVT_EXIT_BREAK then -- exit script
 188     return 2
 189   elseif event == EVT_ENTER_BREAK then -- toggle editing/selecting current field
 190     if fields[current][4] ~= nil then
 191       edit = not edit
 192       if edit == false then
 193         updateField(fields[current])
 194       end
 195     end
 196   elseif edit then
 197     if event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT or event == EVT_PLUS_REPT then
 198       addField(1)
 199     elseif event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then
 200       addField(-1)
 201     end
 202   else
 203     if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT then
 204       selectField(1)
 205     elseif event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT then
 206       selectField(-1)
 207     end
 208   end
 209   redrawFieldsPage()
 210   return 0
 211 end
 212
 213 local calibrationPositionsBitmaps = { "bmp/up.bmp", "bmp/down.bmp", "bmp/left.bmp", "bmp/right.bmp", "bmp/forward.bmp", "bmp/back.bmp"  }
 214
 215 local function runCalibrationPage(event)
 216   fields = calibrationFields
 217   if refreshIndex == #fields then
 218     refreshIndex = 0
 219   end
 220   lcd.clear()
 221   lcd.drawScreenTitle("SxR", page, #pages)
 222   if(calibrationStep < 6) then
 223     lcd.drawText(0, 9, "Turn the SxR as shown", 0)
 224     lcd.drawPixmap(10, 19, calibrationPositionsBitmaps[1 + calibrationStep])
 225     for index = 1, 3, 1 do
 226       local field = fields[index]
 227       lcd.drawText(80, 12+10*index, field[1], 0)
 228       lcd.drawNumber(90, 12+10*index, field[4]/10, LEFT+PREC2)
 229     end
 230
 231     local attr = calibrationState == 0 and INVERS or 0
 232     lcd.drawText(0, 56, "Press [Enter] when ready", attr)
 233   else
 234     lcd.drawText(0, 9, "Calibration completed", 0)
 235     lcd.drawPixmap(10, 19, "bmp/done.bmp")
 236     lcd.drawText(0, 56, "Press [Exit] when ready", attr)
 237   end
 238   if calibrationStep > 6 and (event == EVT_ENTER_BREAK or event == EVT_EXIT_BREAK) then
 239     return 2
 240   elseif event == EVT_ENTER_BREAK then
 241     calibrationState = 1
 242   elseif event == EVT_EXIT_BREAK then
 243     if calibrationStep > 0 then
 244       calibrationStep = 0
 245     end
 246   end
 247   return 0
 248 end
 249
 250 -- Init
 251 local function init()
 252   current, edit, refreshState, refreshIndex = 1, false, 0, 0
 253   pages = {
 254     runWarningPage,
 255     runCalibrationPage
 256   }
 257 end
 258
 259 -- Main
 260 local function run(event)
 261   if event == nil then
 262     error("Cannot be run as a model script!")
 263     return 2
 264   elseif event == EVT_PAGE_BREAK then
 265     selectPage(1)
 266   elseif event == EVT_PAGE_LONG then
 267     killEvents(event);
 268     selectPage(-1)
 269   end
 270
 271   local result = pages[page](event)
 272   refreshNext()
 273
 274   return result
 275 end
 276
 277 return { init=init, run=run }