radio/sdcard/taranis-x7/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 local positionConfirmed = 0
  34 local orientationAutoSense = 0
  35
  36 local calibrationFields = {
  37   {"X:", VALUE, 0x9E, 0, -100, 100, "%"},
  38   {"Y:", VALUE, 0x9F, 0, -100, 100, "%"},
  39   {"Z:", VALUE, 0xA0, 0, -100, 100, "%"}
  40 }
  41
  42 local function drawProgressBar()
  43   local width = (140 * refreshIndex) / #fields
  44   lcd.drawRectangle(30, 1, 144, 6)
  45   lcd.drawFilledRectangle(32, 3, width, 2);
  46 end
  47
  48 -- Select the next or previous page
  49 local function selectPage(step)
  50   page = 1 + ((page + step - 1 + #pages) % #pages)
  51   refreshIndex = 0
  52   calibrationStep = 0
  53   pageOffset = 0
  54 end
  55
  56 -- Draw initial warning page
  57 local function runWarningPage(event)
  58   lcd.clear()
  59   lcd.drawScreenTitle("S6R Calibration", page, #pages)
  60   lcd.drawText(0, 10, "You only need to calibrate", SMLSIZE)
  61   lcd.drawText(0, 20, "once. You will need the S6R,", SMLSIZE)
  62   lcd.drawText(0, 30, "power, and a level surface.", SMLSIZE)
  63   lcd.drawText(0, 40, "Press [Enter] when ready", SMLSIZE)
  64   lcd.drawText(0, 50, "Press [Exit] to cancel", SMLSIZE)
  65   if event == EVT_ENTER_BREAK then
  66     selectPage(1)
  67     return 0
  68   elseif event == EVT_EXIT_BREAK then
  69     return 2
  70   end
  71   return 0
  72 end
  73
  74 -- Redraw the current page
  75 local function redrawFieldsPage()
  76   lcd.clear()
  77   lcd.drawScreenTitle("S6R Calibration", page, #pages)
  78
  79   if refreshIndex < #fields then
  80     drawProgressBar()
  81   end
  82
  83   for index = 1, 7, 1 do
  84     local field = fields[pageOffset+index]
  85     if field == nil then
  86       break
  87     end
  88
  89     local attr = current == (pageOffset+index) and ((edit == true and BLINK or 0) + INVERS) or 0
  90
  91     lcd.drawText(0, 1+8*index, field[1])
  92
  93     if field[4] == nil then
  94       lcd.drawText(COLUMN_2, 1+8*index, "---", attr)
  95     else
  96       if field[2] == VALUE then
  97         lcd.drawNumber(COLUMN_2, 1+8*index, field[4], LEFT + attr)
  98       elseif field[2] == COMBO then
  99         if field[4] >= 0 and field[4] < #(field[5]) then
 100           lcd.drawText(COLUMN_2, 1+8*index, field[5][1+field[4]], attr)
 101         end
 102       end
 103     end
 104   end
 105 end
 106
 107 local function telemetryRead(field)
 108   return sportTelemetryPush(0x17, 0x30, 0x0C30, field)
 109 end
 110
 111 local function telemetryWrite(field, value)
 112   return sportTelemetryPush(0x17, 0x31, 0x0C30, field + value*256)
 113 end
 114
 115 local telemetryPopTimeout = 0
 116 local function refreshNext()
 117   if refreshState == 0 then
 118     if calibrationState == 1 then
 119       if telemetryWrite(0x9D, calibrationStep) == true then
 120         refreshState = 1
 121         calibrationState = 2
 122         telemetryPopTimeout = getTime() + 80 -- normal delay is 500ms
 123       end
 124     elseif #modifications > 0 then
 125       telemetryWrite(modifications[1][1], modifications[1][2])
 126       modifications[1] = nil
 127     elseif refreshIndex < #fields then
 128       local field = fields[refreshIndex + 1]
 129       if telemetryRead(field[3]) == true then
 130         refreshState = 1
 131         telemetryPopTimeout = getTime() + 80 -- normal delay is 500ms
 132       end
 133     end
 134   elseif refreshState == 1 then
 135     local physicalId, primId, dataId, value = sportTelemetryPop()
 136     if physicalId == 0x1A and primId == 0x32 and dataId == 0x0C30 then
 137       local fieldId = value % 256
 138       if calibrationState == 2 then
 139         if fieldId == 0x9D then
 140           refreshState = 0
 141           calibrationState = 0
 142           calibrationStep = (calibrationStep + 1) % 7
 143         end
 144       else
 145         local field = fields[refreshIndex + 1]
 146         if fieldId == field[3] then
 147           local value = math.floor(value / 256)
 148           if field[3] >= 0x9E and field[3] <= 0xA0 then
 149             local b1 = value % 256
 150             local b2 = math.floor(value / 256)
 151             value = b1*256 + b2
 152             value = value - bit32.band(value, 0x8000) * 2
 153           end
 154           if field[2] == COMBO and #field == 6 then
 155             for index = 1, #(field[6]), 1 do
 156               if value == field[6][index] then
 157                 value = index - 1
 158                 break
 159               end
 160             end
 161           elseif field[2] == VALUE and #field == 8 then
 162             value = value - field[8] + field[5]
 163           end
 164           fields[refreshIndex + 1][4] = value
 165           refreshIndex = refreshIndex + 1
 166           refreshState = 0
 167         end
 168       end
 169     elseif getTime() > telemetryPopTimeout then
 170       refreshState = 0
 171       calibrationState = 0
 172     end
 173   end
 174 end
 175
 176 local function updateField(field)
 177   local value = field[4]
 178   if field[2] == COMBO and #field == 6 then
 179     value = field[6][1+value]
 180   elseif field[2] == VALUE and #field == 8 then
 181     value = value + field[8] - field[5]
 182   end
 183   modifications[#modifications+1] = { field[3], value }
 184 end
 185
 186 -- Main
 187 local function runFieldsPage(event)
 188   if event == EVT_EXIT_BREAK then -- exit script
 189     return 2
 190   elseif event == EVT_ENTER_BREAK then -- toggle editing/selecting current field
 191     if fields[current][4] ~= nil then
 192       edit = not edit
 193       if edit == false then
 194         updateField(fields[current])
 195       end
 196     end
 197   elseif edit then
 198     if event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT or event == EVT_PLUS_REPT then
 199       addField(1)
 200     elseif event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT then
 201       addField(-1)
 202     end
 203   else
 204     if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT then
 205       selectField(1)
 206     elseif event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT then
 207       selectField(-1)
 208     end
 209   end
 210   redrawFieldsPage()
 211   return 0
 212 end
 213
 214 local function drawCalibrationOrientation(x, y, step)
 215     local orientation = { {"Label up.", "", 0, 0, 1000, 0, 0, 1000},
 216                             {"Label down.", "", 0, 0, -1000, 0, 0, -1000},
 217                             {"Pins Up.", "", -1000, 0, 0, 1000, 0, 0},
 218                             {"Pins Down.", "", 1000, 0, 0, -1000, 0, 0},
 219                             {"Label facing you", "Pins Right", 0, 1000, 0, 0, -1000, 0},
 220                             {"Label facing you", "Pins Left", 0, -1000 , 0, 0, 1000, 0} }
 221
 222     lcd.drawText(0, 9, "Place the S6R as follows:", 0)
 223     lcd.drawText(x-9, y, orientation[step][1])
 224     lcd.drawText(x-9, y+10, orientation[step][2])
 225     local positionStatus = 0
 226     for index = 1, 3, 1 do
 227       local field = fields[index]
 228       lcd.drawText(90, 12+10*index, field[1], 0)
 229       if math.abs(field[4] - orientation[step][2+index+orientationAutoSense]) < 200 then
 230         lcd.drawNumber(100, 12+10*index, field[4]/10, LEFT+PREC2)
 231         positionStatus = positionStatus + 1
 232       else
 233         lcd.drawNumber(100, 12+10*index, field[4]/10, LEFT+PREC2+INVERS)
 234       end
 235     end
 236     if step == 3 and positionStatus == 2 then -- orientation auto sensing
 237       orientationAutoSense = 3 - orientationAutoSense
 238     end
 239     if positionStatus == 3 then
 240       lcd.drawText(0, 56, " [Enter] to validate          ", INVERS)
 241       positionConfirmed = 1
 242     end
 243 end
 244
 245 local function runCalibrationPage(event)
 246   fields = calibrationFields
 247   if refreshIndex == #fields then
 248     refreshIndex = 0
 249   end
 250   lcd.clear()
 251   lcd.drawScreenTitle("S6R Calibration", page, #pages)
 252   if(calibrationStep < 6) then
 253     drawCalibrationOrientation(10, 24, 1 + calibrationStep)
 254
 255     local attr = calibrationState == 0 and INVERS or 0
 256     --lcd.drawText(0, 56, "[Enter] to validate", attr)
 257   else
 258     lcd.drawText(0, 19, "Calibration completed", 0)
 259 --    lcd.drawText(10, 19, "Done",0)
 260     lcd.drawText(0, 56, "Press [Exit] when ready", attr)
 261   end
 262   if calibrationStep > 6 and (event == EVT_ENTER_BREAK or event == EVT_EXIT_BREAK) then
 263     return 2
 264   elseif event == EVT_ENTER_BREAK and positionConfirmed  then
 265     calibrationState = 1
 266     positionConfirmed = 0
 267   end
 268   return 0
 269 end
 270
 271
 272 -- Init
 273 local function init()
 274   current, edit, refreshState, refreshIndex = 1, false, 0, 0
 275   pages = {
 276     runWarningPage,
 277     runCalibrationPage
 278   }
 279 end
 280
 281 -- Main
 282 local function run(event)
 283   if event == nil then
 284     error("Cannot be run as a model script!")
 285     return 2
 286   elseif event == EVT_PAGE_BREAK then
 287     selectPage(1)
 288   elseif event == EVT_PAGE_LONG then
 289     killEvents(event);
 290     selectPage(-1)
 291   end
 292
 293   local result = pages[page](event)
 294   refreshNext()
 295
 296   return result
 297 end
 298
 299 return { init=init, run=run }