diff --git a/radio/src/cli.cpp b/radio/src/cli.cpp index e38f41f6b..edcb0ac9c 100644 --- a/radio/src/cli.cpp +++ b/radio/src/cli.cpp @@ -404,7 +404,7 @@ int cliShowJitter(const char ** argv) { serialPrint( "# anaIn rawJ avgJ"); for (int i=0; icount; j++) { diff --git a/radio/src/gui/9x/menu_general_calib.cpp b/radio/src/gui/9x/menu_general_calib.cpp index 7baee3dbb..6587aef98 100644 --- a/radio/src/gui/9x/menu_general_calib.cpp +++ b/radio/src/gui/9x/menu_general_calib.cpp @@ -23,6 +23,14 @@ #define XPOT_DELTA 10 #define XPOT_DELAY 10 /* cycles */ +enum CalibrationState { + CALIB_START = 0, + CALIB_SET_MIDPOINT, + CALIB_MOVE_STICKS, + CALIB_STORE, + CALIB_FINISHED +}; + void menuCommonCalib(uint8_t event) { for (uint8_t i=0; i> 1; + int16_t vt = anaIn(i); reusableBuffer.calib.loVals[i] = min(vt, reusableBuffer.calib.loVals[i]); reusableBuffer.calib.hiVals[i] = max(vt, reusableBuffer.calib.hiVals[i]); if (i >= POT1 && i <= POT_LAST) { @@ -72,6 +80,8 @@ bool menuCommonCalib(evt_t event) uint8_t idx = i - POT1; int count = reusableBuffer.calib.xpotsCalib[idx].stepsCount; if (IS_POT_MULTIPOS(i) && count <= XPOTS_MULTIPOS_COUNT) { + // use raw analog value for multipos calibraton, anaIn() already has multipos decoded value + vt = getAnalogValue(i) >> 1; if (reusableBuffer.calib.xpotsCalib[idx].lastCount == 0 || vt < reusableBuffer.calib.xpotsCalib[idx].lastPosition - XPOT_DELTA || vt > reusableBuffer.calib.xpotsCalib[idx].lastPosition + XPOT_DELTA) { reusableBuffer.calib.xpotsCalib[idx].lastPosition = vt; reusableBuffer.calib.xpotsCalib[idx].lastCount = 1; @@ -103,7 +113,7 @@ bool menuCommonCalib(evt_t event) switch (event) { case EVT_ENTRY: case EVT_KEY_BREAK(KEY_EXIT): - calibrationState = 0; + calibrationState = CALIB_START; break; case EVT_KEY_BREAK(KEY_ENTER): @@ -112,7 +122,7 @@ bool menuCommonCalib(evt_t event) } switch (calibrationState) { - case 0: + case CALIB_START: // START CALIBRATION if (!READ_ONLY()) { lcdDrawText(50, 3, STR_MENUCALIBRATION, MENU_TITLE_COLOR); @@ -120,14 +130,14 @@ bool menuCommonCalib(evt_t event) } break; - case 1: + case CALIB_SET_MIDPOINT: // SET MIDPOINT lcdDrawText(50, 3, STR_MENUCALIBRATION, MENU_TITLE_COLOR); lcdDrawText(50, 3+FH, "Please center sticks and press [Enter]", MENU_TITLE_COLOR); for (int i=0; i> 1; + reusableBuffer.calib.midVals[i] = anaIn(i); if (i < NUM_XPOTS) { reusableBuffer.calib.xpotsCalib[i].stepsCount = 0; reusableBuffer.calib.xpotsCalib[i].lastCount = 0; @@ -135,7 +145,7 @@ bool menuCommonCalib(evt_t event) } break; - case 2: + case CALIB_MOVE_STICKS: // MOVE STICKS/POTS lcdDrawText(50, 3, STR_MENUCALIBRATION, MENU_TITLE_COLOR); lcdDrawText(50, 3+FH, "Move sticks, pots and sliders and press [Enter]", MENU_TITLE_COLOR); @@ -173,14 +183,14 @@ bool menuCommonCalib(evt_t event) } break; - case 3: + case CALIB_STORE: g_eeGeneral.chkSum = evalChkSum(); storageDirty(EE_GENERAL); - calibrationState = 4; + calibrationState = CALIB_FINISHED; break; default: - calibrationState = 0; + calibrationState = CALIB_START; break; } @@ -194,8 +204,8 @@ bool menuCommonCalib(evt_t event) bool menuGeneralCalib(evt_t event) { if (event == EVT_ENTRY || event == EVT_ENTRY_UP) TRACE("Menu %s displayed ...", STR_MENUCALIBRATION); - if (calibrationState == 4) { - calibrationState = 0; + if (calibrationState == CALIB_FINISHED) { + calibrationState = CALIB_START; popMenu(); return false; } @@ -210,8 +220,8 @@ bool menuGeneralCalib(evt_t event) bool menuFirstCalib(evt_t event) { - if (event == EVT_KEY_BREAK(KEY_EXIT) || calibrationState == 4) { - calibrationState = 0; + if (event == EVT_KEY_BREAK(KEY_EXIT) || calibrationState == CALIB_FINISHED) { + calibrationState = CALIB_START; chainMenu(menuMainView); return false; } diff --git a/radio/src/gui/taranis/menu_general_calib.cpp b/radio/src/gui/taranis/menu_general_calib.cpp index fa89b0998..c9c62bd64 100644 --- a/radio/src/gui/taranis/menu_general_calib.cpp +++ b/radio/src/gui/taranis/menu_general_calib.cpp @@ -25,6 +25,14 @@ #define BAR_SPACING 12 #define BAR_HEIGHT 22 +enum CalibrationState { + CALIB_START = 0, + CALIB_SET_MIDPOINT, + CALIB_MOVE_STICKS, + CALIB_STORE, + CALIB_FINISHED +}; + void drawPotsBars() { // Optimization by Mike Blandford @@ -41,7 +49,7 @@ void drawPotsBars() void menuCommonCalib(uint8_t event) { for (uint8_t i=0; i> 1; + int16_t vt = anaIn(i); reusableBuffer.calib.loVals[i] = min(vt, reusableBuffer.calib.loVals[i]); reusableBuffer.calib.hiVals[i] = max(vt, reusableBuffer.calib.hiVals[i]); if (i >= POT1 && i <= POT_LAST) { @@ -51,6 +59,8 @@ void menuCommonCalib(uint8_t event) uint8_t idx = i - POT1; int count = reusableBuffer.calib.xpotsCalib[idx].stepsCount; if (IS_POT_MULTIPOS(i) && count <= XPOTS_MULTIPOS_COUNT) { + // use raw analog value for multipos calibraton, anaIn() already has multipos decoded value + vt = getAnalogValue(i) >> 1; if (reusableBuffer.calib.xpotsCalib[idx].lastCount == 0 || vt < reusableBuffer.calib.xpotsCalib[idx].lastPosition - XPOT_DELTA || vt > reusableBuffer.calib.xpotsCalib[idx].lastPosition + XPOT_DELTA) { reusableBuffer.calib.xpotsCalib[idx].lastPosition = vt; reusableBuffer.calib.xpotsCalib[idx].lastCount = 1; @@ -84,7 +94,7 @@ void menuCommonCalib(uint8_t event) switch (event) { case EVT_ENTRY: case EVT_KEY_BREAK(KEY_EXIT): - reusableBuffer.calib.state = 0; + reusableBuffer.calib.state = CALIB_START; break; case EVT_KEY_BREAK(KEY_ENTER): @@ -93,14 +103,14 @@ void menuCommonCalib(uint8_t event) } switch (reusableBuffer.calib.state) { - case 0: + case CALIB_START: // START CALIBRATION if (!READ_ONLY()) { lcd_putsLeft(MENU_HEADER_HEIGHT+2*FH, STR_MENUTOSTART); } break; - case 1: + case CALIB_SET_MIDPOINT: // SET MIDPOINT lcdDrawText(0*FW, MENU_HEADER_HEIGHT+FH, STR_SETMIDPOINT, INVERS); lcd_putsLeft(MENU_HEADER_HEIGHT+2*FH, STR_MENUWHENDONE); @@ -116,7 +126,7 @@ void menuCommonCalib(uint8_t event) } break; - case 2: + case CALIB_MOVE_STICKS: // MOVE STICKS/POTS STICK_SCROLL_DISABLE(); lcdDrawText(0*FW, MENU_HEADER_HEIGHT+FH, STR_MOVESTICKSPOTS, INVERS); @@ -133,7 +143,7 @@ void menuCommonCalib(uint8_t event) } break; - case 3: + case CALIB_STORE: for (uint8_t i=POT1; i<=POT_LAST; i++) { int idx = i - POT1; int count = reusableBuffer.calib.xpotsCalib[idx].stepsCount; @@ -159,11 +169,11 @@ void menuCommonCalib(uint8_t event) } g_eeGeneral.chkSum = evalChkSum(); storageDirty(EE_GENERAL); - reusableBuffer.calib.state = 4; + reusableBuffer.calib.state = CALIB_FINISHED; break; default: - reusableBuffer.calib.state = 0; + reusableBuffer.calib.state = CALIB_START; break; } @@ -173,7 +183,7 @@ void menuCommonCalib(uint8_t event) #if 0 for (int i=POT1; i<=POT_LAST; i++) { uint8_t steps = 0; - if (reusableBuffer.calib.state == 2) { + if (reusableBuffer.calib.state == CALIB_MOVE_STICKS) { steps = reusableBuffer.calib.xpotsCalib[i-POT1].stepsCount; } else if (IS_POT_MULTIPOS(i)) { @@ -192,14 +202,14 @@ void menuGeneralCalib(uint8_t event) check_simple(STR_MENUCALIBRATION, event, e_Calib, menuTabGeneral, DIM(menuTabGeneral), 0); menuCommonCalib(READ_ONLY() ? 0 : event); if (menuEvent) { - calibrationState = 0; + calibrationState = CALIB_START; } } void menuFirstCalib(uint8_t event) { - if (event == EVT_KEY_BREAK(KEY_EXIT) || reusableBuffer.calib.state == 4) { - calibrationState = 0; + if (event == EVT_KEY_BREAK(KEY_EXIT) || reusableBuffer.calib.state == CALIB_FINISHED) { + calibrationState = CALIB_START; chainMenu(menuMainView); } else { diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index 55ea6eaa5..95cbeef34 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -1487,21 +1487,40 @@ uint16_t BandGap ; #endif #if defined(JITTER_MEASURE) -JitterMeter rawJitter[NUMBER_ANALOG]; -JitterMeter avgJitter[NUMBER_ANALOG]; -tmr10ms_t jitterResetTime = 0; + JitterMeter rawJitter[NUMBER_ANALOG]; + JitterMeter avgJitter[NUMBER_ANALOG]; + tmr10ms_t jitterResetTime = 0; #if defined(PCBTARANIS) #define JITTER_MEASURE_ACTIVE() (menuHandlers[menuLevel] == menuGeneralDiagAna) #elif defined(CLI) #define JITTER_MEASURE_ACTIVE() (1) #endif -#endif // defined(JITTER_MEASURE) +#endif + + +#if defined(VIRTUALINPUTS) && defined(JITTER_FILTER) + #define JITTER_FILTER_STRENGTH 4 // tune this value, bigger value - more filtering (range: 1-5) (see explanation below) + #define ANALOG_SCALE 1 // tune this value, bigger value - more filtering (range: 0-3) (see explanation below) + + #define JITTER_ALPHA (1< 32) + #error "JITTER_FILTER_STRENGTH and ANALOG_SCALE are too big, their summ should be <= 5 !!!" + #endif +#else + #define ANALOG_SCALE 0 + #define JITTER_ALPHA 1 + #define ANALOG_MULTIPLIER 1 + #define ANA_FILT(chan) (s_anaFilt[chan]) +#endif + #if !defined(SIMU) uint16_t anaIn(uint8_t chan) { #if defined(VIRTUALINPUTS) - return s_anaFilt[chan]; + return ANA_FILT(chan); #elif defined(PCBSKY9X) && !defined(REVA) static const uint8_t crossAna[]={1,5,7,0,4,6,2,3}; if (chan == TX_CURRENT) { @@ -1535,6 +1554,7 @@ void getADC() #if defined(JITTER_MEASURE) if (JITTER_MEASURE_ACTIVE() && jitterResetTime < get_tmr10ms()) { + // reset jitter measurement every second for (uint32_t x=0; x> 3; + for (uint32_t x=0; x> (3 - ANALOG_SCALE); -#if defined(JITTER_FILTER) - // jitter filter - uint16_t diff = (v > s_anaFilt[x]) ? (v - s_anaFilt[x]) : (s_anaFilt[x] - v); - if (diff < 10) { - // apply filter - v = (7 * s_anaFilt[x] + v) >> 3; - } -#endif - -#if defined(JITTER_MEASURE) - if (JITTER_MEASURE_ACTIVE()) { - avgJitter[x].measure(v); - } -#endif - -#if defined(VIRTUALINPUTS) - StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[x]; - if (IS_POT_MULTIPOS(x) && calib->count>0 && calib->count> 4); - s_anaFilt[x] = 2*RESX; - for (int i=0; icount; i++) { - if (vShifted < calib->steps[i]) { - s_anaFilt[x] = i*2*RESX/calib->count; - break; - } - } +#if defined(VIRTUALINPUTS) && defined(JITTER_FILTER) + // Jitter filter: + // * pass trough any big change directly + // * for small change use Modified moving average (MMA) filter + // + // Explanation: + // + // Normal MMA filter has this formula: + // = ((ALPHA-1)* + )/ALPHA + // + // If calculation is done this way with integer arithmetics, then any small change in + // input signal is lost. One way to combat that, is to rearrange the formula somewhat, + // to store a more precise (larger) number between iterations. The basic idea is to + // store undivided value between iterations. Therefore an new variable is + // used. The new formula becomes: + // = - /ALPHA + + // = /ALPHA (use only when out is needed) + // + // The above formula with a maximum allowed ALPHA value (we are limited by + // the 16 bit s_anaFilt[]) was tested on the radio. The resulting signal still had + // some jitter (a value of 1 was observed). The jitter might be bigger on other + // radios. + // + // So another idea is to use larger input values for filtering. So instead of using + // input in a range from 0 to 2047, we use twice larger number (temp[x] is divided less) + // + // This also means that ALPHA must be lowered (remember 16 bit limit), but test results + // have proved that this kind of filtering gives better results. So the recommended values + // for filter are: + // JITTER_FILTER_STRENGTH 4 + // ANALOG_SCALE 1 + // + // Variables mapping: + // * = v + // * = s_anaFilt[x] + uint16_t previous = s_anaFilt[x] / JITTER_ALPHA; + uint16_t diff = (v > previous) ? (v - previous) : (previous - v); + if (diff < 10 * ANALOG_MULTIPLIER) { + // apply jitter filter + s_anaFilt[x] = (s_anaFilt[x] - previous) + v; } else #endif { - s_anaFilt[x] = v; + //use unfiltered value + s_anaFilt[x] = v * JITTER_ALPHA; } + +#if defined(JITTER_MEASURE) + if (JITTER_MEASURE_ACTIVE()) { + avgJitter[x].measure(ANA_FILT(x)); + } +#endif + +#if defined(VIRTUALINPUTS) + #define ANAFILT_MAX (2 * RESX * JITTER_ALPHA * ANALOG_MULTIPLIER - 1) + StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[x]; + if (IS_POT_MULTIPOS(x) && IS_MULTIPOS_CALIBRATED(calib)) { + // TODO: consider adding another low pass filter to eliminate multipos switching glitches + uint8_t vShifted = ANA_FILT(x) >> 4; + s_anaFilt[x] = ANAFILT_MAX; + for (uint32_t i=0; icount; i++) { + if (vShifted < calib->steps[i]) { + s_anaFilt[x] = (i * ANAFILT_MAX) / calib->count; + break; + } + } + } +#endif // defined(VIRTUALINPUTS) } } #endif diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 667e417f4..538920ae9 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -394,6 +394,7 @@ void memswap(void * a, void * b, uint8_t size); #endif #define IS_POT(x) ((x)>=POT1 && (x)<=POT_LAST) +#define IS_MULTIPOS_CALIBRATED(cal) (cal->count>0 && cal->countcount>0 && calib->countcount); uint8_t previousPos = potsPos[i] >> 4; uint8_t previousStoredPos = potsPos[i] & 0x0F;