radio/src/gui/212x64/radio_calibration.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23 #define XPOT_DELTA    10
  24 #define XPOT_DELAY    10 /* cycles */
  25 #define BAR_SPACING   12
  26 #define BAR_HEIGHT    22
  27
  28 enum CalibrationState {
  29   CALIB_START = 0,
  30   CALIB_SET_MIDPOINT,
  31   CALIB_MOVE_STICKS,
  32   CALIB_STORE,
  33   CALIB_FINISHED
  34 };
  35
  36 void drawPotsBars()
  37 {
  38   // Optimization by Mike Blandford
  39   for (uint8_t x=LCD_W/2-(NUM_POTS+NUM_SLIDERS)/2*BAR_SPACING+BAR_SPACING/2, i=NUM_STICKS; i<NUM_STICKS+NUM_POTS+NUM_SLIDERS; x+=BAR_SPACING, i++) {
  40     if (IS_POT_SLIDER_AVAILABLE(i)) {
  41       uint8_t len = ((calibratedAnalogs[i]+RESX)*BAR_HEIGHT/(RESX*2))+1l;  // calculate once per loop
  42       V_BAR(x, LCD_H-8, len);
  43       putsStickName(x-2, LCD_H-6, i, TINSIZE);
  44     }
  45   }
  46 }
  47
  48 void menuCommonCalib(event_t event)
  49 {
  50   for (uint8_t i=0; i<NUM_STICKS+NUM_POTS+NUM_SLIDERS; i++) { // get low and high vals for sticks and trims
  51     int16_t vt = anaIn(i);
  52     reusableBuffer.calib.loVals[i] = min(vt, reusableBuffer.calib.loVals[i]);
  53     reusableBuffer.calib.hiVals[i] = max(vt, reusableBuffer.calib.hiVals[i]);
  54     if (i >= POT1 && i <= POT_LAST) {
  55       if (IS_POT_WITHOUT_DETENT(i)) {
  56         reusableBuffer.calib.midVals[i] = (reusableBuffer.calib.hiVals[i] + reusableBuffer.calib.loVals[i]) / 2;
  57       }
  58       uint8_t idx = i - POT1;
  59       int count = reusableBuffer.calib.xpotsCalib[idx].stepsCount;
  60       if (IS_POT_MULTIPOS(i) && count <= XPOTS_MULTIPOS_COUNT) {
  61         // use raw analog value for multipos calibraton, anaIn() already has multipos decoded value
  62         vt = getAnalogValue(i) >> 1;
  63         if (reusableBuffer.calib.xpotsCalib[idx].lastCount == 0 || vt < reusableBuffer.calib.xpotsCalib[idx].lastPosition - XPOT_DELTA || vt > reusableBuffer.calib.xpotsCalib[idx].lastPosition + XPOT_DELTA) {
  64           reusableBuffer.calib.xpotsCalib[idx].lastPosition = vt;
  65           reusableBuffer.calib.xpotsCalib[idx].lastCount = 1;
  66         }
  67         else {
  68           if (reusableBuffer.calib.xpotsCalib[idx].lastCount < 255) reusableBuffer.calib.xpotsCalib[idx].lastCount++;
  69         }
  70         if (reusableBuffer.calib.xpotsCalib[idx].lastCount == XPOT_DELAY) {
  71           int16_t position = reusableBuffer.calib.xpotsCalib[idx].lastPosition;
  72           bool found = false;
  73           for (int j=0; j<count; j++) {
  74             int16_t step = reusableBuffer.calib.xpotsCalib[idx].steps[j];
  75             if (position >= step-XPOT_DELTA && position <= step+XPOT_DELTA) {
  76               found = true;
  77               break;
  78             }
  79           }
  80           if (!found) {
  81             if (count < XPOTS_MULTIPOS_COUNT) {
  82               reusableBuffer.calib.xpotsCalib[idx].steps[count] = position;
  83             }
  84             reusableBuffer.calib.xpotsCalib[idx].stepsCount += 1;
  85           }
  86         }
  87       }
  88     }
  89   }
  90
  91   menuCalibrationState = reusableBuffer.calib.state; // make sure we don't scroll while calibrating
  92
  93   switch (event) {
  94     case EVT_ENTRY:
  95     case EVT_KEY_BREAK(KEY_EXIT):
  96       reusableBuffer.calib.state = CALIB_START;
  97       break;
  98
  99     case EVT_KEY_BREAK(KEY_ENTER):
 100       reusableBuffer.calib.state++;
 101       break;
 102   }
 103
 104   switch (reusableBuffer.calib.state) {
 105     case CALIB_START:
 106       // START CALIBRATION
 107       if (!READ_ONLY()) {
 108         lcdDrawTextAlignedLeft(MENU_HEADER_HEIGHT+2*FH, STR_MENUTOSTART);
 109       }
 110       break;
 111
 112     case CALIB_SET_MIDPOINT:
 113       // SET MIDPOINT
 114       lcdDrawText(0*FW, MENU_HEADER_HEIGHT+FH, STR_SETMIDPOINT, INVERS);
 115       lcdDrawTextAlignedLeft(MENU_HEADER_HEIGHT+2*FH, STR_MENUWHENDONE);
 116       for (uint8_t i=0; i<NUM_STICKS+NUM_POTS+NUM_SLIDERS; i++) {
 117         reusableBuffer.calib.loVals[i] = 15000;
 118         reusableBuffer.calib.hiVals[i] = -15000;
 119         reusableBuffer.calib.midVals[i] = getAnalogValue(i) >> 1;
 120         if (i<NUM_XPOTS) {
 121           reusableBuffer.calib.xpotsCalib[i].stepsCount = 0;
 122           reusableBuffer.calib.xpotsCalib[i].lastCount = 0;
 123         }
 124       }
 125       break;
 126
 127     case CALIB_MOVE_STICKS:
 128       // MOVE STICKS/POTS
 129       STICK_SCROLL_DISABLE();
 130       lcdDrawText(0*FW, MENU_HEADER_HEIGHT+FH, STR_MOVESTICKSPOTS, INVERS);
 131       lcdDrawTextAlignedLeft(MENU_HEADER_HEIGHT+2*FH, STR_MENUWHENDONE);
 132       for (uint8_t i=0; i<NUM_STICKS+NUM_POTS+NUM_SLIDERS; i++) {
 133         if (abs(reusableBuffer.calib.loVals[i]-reusableBuffer.calib.hiVals[i]) > 50) {
 134           g_eeGeneral.calib[i].mid = reusableBuffer.calib.midVals[i];
 135           int16_t v = reusableBuffer.calib.midVals[i] - reusableBuffer.calib.loVals[i];
 136           g_eeGeneral.calib[i].spanNeg = v - v/STICK_TOLERANCE;
 137           v = reusableBuffer.calib.hiVals[i] - reusableBuffer.calib.midVals[i];
 138           g_eeGeneral.calib[i].spanPos = v - v/STICK_TOLERANCE;
 139         }
 140       }
 141       break;
 142
 143     case CALIB_STORE:
 144       for (uint8_t i=POT1; i<=POT_LAST; i++) {
 145         int idx = i - POT1;
 146         int count = reusableBuffer.calib.xpotsCalib[idx].stepsCount;
 147         if (IS_POT_MULTIPOS(i)) {
 148           if (count > 1 && count <= XPOTS_MULTIPOS_COUNT) {
 149             for (int j=0; j<count; j++) {
 150               for (int k=j+1; k<count; k++) {
 151                 if (reusableBuffer.calib.xpotsCalib[idx].steps[k] < reusableBuffer.calib.xpotsCalib[idx].steps[j]) {
 152                   SWAP(reusableBuffer.calib.xpotsCalib[idx].steps[j], reusableBuffer.calib.xpotsCalib[idx].steps[k]);
 153                 }
 154               }
 155             }
 156             StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[i];
 157             calib->count = count - 1;
 158             for (int j=0; j<calib->count; j++) {
 159               calib->steps[j] = (reusableBuffer.calib.xpotsCalib[idx].steps[j+1] + reusableBuffer.calib.xpotsCalib[idx].steps[j]) >> 5;
 160             }
 161           }
 162           else {
 163             g_eeGeneral.potsConfig &= ~(0x03<<(2*idx));
 164           }
 165         }
 166       }
 167       g_eeGeneral.chkSum = evalChkSum();
 168       storageDirty(EE_GENERAL);
 169       reusableBuffer.calib.state = CALIB_FINISHED;
 170       break;
 171
 172     default:
 173       reusableBuffer.calib.state = CALIB_START;
 174       break;
 175   }
 176
 177   doMainScreenGraphics();
 178   drawPotsBars();
 179
 180 #if 0
 181   for (int i=POT1; i<=POT_LAST; i++) {
 182     uint8_t steps = 0;
 183     if (reusableBuffer.calib.state == CALIB_MOVE_STICKS) {
 184       steps = reusableBuffer.calib.xpotsCalib[i-POT1].stepsCount;
 185     }
 186     else if (IS_POT_MULTIPOS(i)) {
 187       StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[i];
 188       steps = calib->count + 1;
 189     }
 190     if (steps > 0 && steps <= XPOTS_MULTIPOS_COUNT) {
 191       lcdDrawNumber(LCD_W/2-2+(i-POT1)*5, LCD_H-6, steps, TINSIZE|RIGHT);
 192     }
 193   }
 194 #endif
 195 }
 196
 197 void menuRadioCalibration(event_t event)
 198 {
 199   check_simple(STR_MENUCALIBRATION, event, MENU_RADIO_CALIBRATION, menuTabGeneral, DIM(menuTabGeneral), 0);
 200   menuCommonCalib(READ_ONLY() ? 0 : event);
 201   if (menuEvent) {
 202     menuCalibrationState = CALIB_START;
 203   }
 204 }
 205
 206 void menuFirstCalib(event_t event)
 207 {
 208   if (event == EVT_KEY_BREAK(KEY_EXIT) || reusableBuffer.calib.state == CALIB_FINISHED) {
 209     menuCalibrationState = CALIB_START;
 210     chainMenu(menuMainView);
 211   }
 212   else {
 213     lcdDrawTextAlignedCenter(0*FH, MENUCALIBRATION);
 214     lcdInvertLine(0);
 215     menuCommonCalib(event);
 216   }
 217 }