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("SxR Calibration", page, #pages)
  60   lcd.drawText(0, 10, "You only need to calibrate", SMLSIZE)
  61   lcd.drawText(0, 20, "once. You will need the SxR,", 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("SxR 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           value =  bit32.band(value, 0xffff)
 149           if field[3] >= 0x9E and field[3] <= 0xA0 then
 150             local b1 = value % 256
 151             local b2 = math.floor(value / 256)
 152             value = b1*256 + b2
 153             value = value - bit32.band(value, 0x8000) * 2
 154           end
 155           if field[2] == COMBO and #field == 6 then
 156             for index = 1, #(field[6]), 1 do
 157               if value == field[6][index] then
 158                 value = index - 1
 159                 break
 160               end
 161             end
 162           elseif field[2] == VALUE and #field == 8 then
 163             value = value - field[8] + field[5]
 164           end
 165           fields[refreshIndex + 1][4] = value
 166           refreshIndex = refreshIndex + 1
 167           refreshState = 0
 168         end
 169       end
 170     elseif getTime() > telemetryPopTimeout then
 171       refreshState = 0
 172       calibrationState = 0
 173     end
 174   end
 175 end
 176
 177 local function updateField(field)
 178   local value = field[4]
 179   if field[2] == COMBO and #field == 6 then
 180     value = field[6][1+value]
 181   elseif field[2] == VALUE and #field == 8 then
 182     value = value + field[8] - field[5]
 183   end
 184   modifications[#modifications+1] = { field[3], value }
 185 end
 186
 187 -- Main
 188 local function runFieldsPage(event)
 189   if event == EVT_EXIT_BREAK then -- exit script
 190     return 2
 191   elseif event == EVT_ENTER_BREAK then -- toggle editing/selecting current field
 192     if fields[current][4] ~= nil then
 193       edit = not edit
 194       if edit == false then
 195         updateField(fields[current])
 196       end
 197     end
 198   elseif edit then
 199     if event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT or event == EVT_PLUS_REPT or event == EVT_DOWN_BREAK then
 200       addField(1)
 201     elseif event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_MINUS_REPT or event == EVT_UP_BREAK then
 202       addField(-1)
 203     end
 204   else
 205     if event == EVT_MINUS_FIRST or event == EVT_ROT_LEFT or event == EVT_UP_BREAK then
 206       selectField(1)
 207     elseif event == EVT_PLUS_FIRST or event == EVT_ROT_RIGHT or event == EVT_DOWN_BREAK then
 208       selectField(-1)
 209     end
 210   end
 211   redrawFieldsPage()
 212   return 0
 213 end
 214
 215 local function drawCalibrationOrientation(x, y, step)
 216     local orientation = { {"Label up.", "", 0, 0, 1000, 0, 0, 1000},
 217                             {"Label down.", "", 0, 0, -1000, 0, 0, -1000},
 218                             {"Pins Up.", "", -1000, 0, 0, 1000, 0, 0},
 219                             {"Pins Down.", "", 1000, 0, 0, -1000, 0, 0},
 220                             {"Label facing you", "Pins Right", 0, 1000, 0, 0, -1000, 0},
 221                             {"Label facing you", "Pins Left", 0, -1000 , 0, 0, 1000, 0} }
 222
 223     lcd.drawText(0, 9, "Place the SxR as follows:", 0)
 224     lcd.drawText(x-9, y, orientation[step][1])
 225     lcd.drawText(x-9, y+10, orientation[step][2])
 226     local positionStatus = 0
 227     for index = 1, 3, 1 do
 228       local field = fields[index]
 229       lcd.drawText(90, 12+10*index, field[1], 0)
 230       if math.abs(field[4] - orientation[step][2+index+orientationAutoSense]) < 200 then
 231         lcd.drawNumber(100, 12+10*index, field[4]/10, LEFT+PREC2)
 232         positionStatus = positionStatus + 1
 233       else
 234         lcd.drawNumber(100, 12+10*index, field[4]/10, LEFT+PREC2+INVERS)
 235       end
 236     end
 237     if step == 3 and positionStatus == 2 then -- orientation auto sensing
 238       orientationAutoSense = 3 - orientationAutoSense
 239     end
 240     if positionStatus == 3 then
 241       lcd.drawText(0, 56, " [Enter] to validate          ", INVERS)
 242       positionConfirmed = 1
 243     end
 244 end
 245
 246 local function runCalibrationPage(event)
 247   fields = calibrationFields
 248   if refreshIndex == #fields then
 249     refreshIndex = 0
 250   end
 251   lcd.clear()
 252   lcd.drawScreenTitle("SxR Calibration", page, #pages)
 253   if(calibrationStep < 6) then
 254     drawCalibrationOrientation(10, 24, 1 + calibrationStep)
 255
 256     local attr = calibrationState == 0 and INVERS or 0
 257     --lcd.drawText(0, 56, "[Enter] to validate", attr)
 258   else
 259     lcd.drawText(0, 19, "Calibration completed", 0)
 260 --    lcd.drawText(10, 19, "Done",0)
 261     lcd.drawText(0, 56, "Press [Exit] when ready", attr)
 262   end
 263   if calibrationStep > 6 and (event == EVT_ENTER_BREAK or event == EVT_EXIT_BREAK) then
 264     return 2
 265   elseif event == EVT_ENTER_BREAK and positionConfirmed  then
 266     calibrationState = 1
 267     positionConfirmed = 0
 268   end
 269   return 0
 270 end
 271
 272
 273 -- Init
 274 local function init()
 275   current, edit, refreshState, refreshIndex = 1, false, 0, 0
 276   pages = {
 277     runWarningPage,
 278     runCalibrationPage
 279   }
 280 end
 281
 282 -- Main
 283 local function run(event)
 284   if event == nil then
 285     error("Cannot be run as a model script!")
 286     return 2
 287   elseif event == EVT_PAGE_BREAK or event==EVT_RIGHT_BREAK then
 288     selectPage(1)
 289   elseif event == EVT_PAGE_LONG or event==EVT_LEFT_BREAK then
 290     killEvents(event);
 291     selectPage(-1)
 292   end
 293
 294   local result = pages[page](event)
 295   refreshNext()
 296
 297   return result
 298 end
 299
 300 return { init=init, run=run }