Re #3233: Better ADC jitter filter: using inverted formula and scaled input value for jitter filter, gives better result for small changes

radio/src/cli.cpp

@@ -404,7 +404,7 @@ int cliShowJitter(const char ** argv)
 {
   serialPrint(  "#   anaIn   rawJ   avgJ");
   for (int i=0; i<NUMBER_ANALOG; i++) {
-    serialPrint("A%02d %04X %3d %3d", i, anaIn(i), rawJitter[i].get(), avgJitter[i].get());
+    serialPrint("A%02d %04X %04X %3d %3d", i, getAnalogValue(i), anaIn(i), rawJitter[i].get(), avgJitter[i].get());
     if (IS_POT_MULTIPOS(i)) {
       StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[i];
       for (int j=0; j<calib->count; j++) {

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<NUM_STICKS+NUM_POTS; i++) { // get low and high vals for sticks and trims
@@ -41,7 +49,7 @@ void menuCommonCalib(uint8_t event)
   switch (event)
   {
     case EVT_ENTRY:
-      reusableBuffer.calib.state = 0;
+      reusableBuffer.calib.state = CALIB_START;
       break;
 
     case EVT_KEY_BREAK(KEY_ENTER):
@@ -50,14 +58,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);
@@ -69,7 +77,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);
@@ -86,14 +94,14 @@ void menuCommonCalib(uint8_t event)
       }
       break;
 
-    case 3:
+    case CALIB_STORE:
       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;
   }
 
@@ -105,7 +113,7 @@ void menuGeneralCalib(uint8_t event)
   check_simple(event, e_Calib, menuTabGeneral, DIM(menuTabGeneral), 0);
 
   if (menuEvent) {
-    calibrationState = 0;
+    calibrationState = CALIB_START;
   }
 
   TITLE(STR_MENUCALIBRATION);
@@ -114,8 +122,8 @@ void menuGeneralCalib(uint8_t event)
 
 void menuFirstCalib(uint8_t event)
 {
-  if (event == EVT_KEY_BREAK(KEY_EXIT) || reusableBuffer.calib.state == 4) {
+  if (event == EVT_KEY_BREAK(KEY_EXIT) || reusableBuffer.calib.state == CALIB_FINISHED) {
-    calibrationState = 0;
+    calibrationState = CALIB_START;
     chainMenu(menuMainView);
   }
   else {

radio/src/gui/horus/menu_general_calib.cpp

@@ -29,6 +29,14 @@
 #define STICK_LEFT_X                   25
 #define STICK_RIGHT_X                  (LCD_W-STICK_LEFT_X-STICKS_WIDTH)
 
+enum CalibrationState {
+  CALIB_START = 0,
+  CALIB_SET_MIDPOINT,
+  CALIB_MOVE_STICKS,
+  CALIB_STORE,
+  CALIB_FINISHED
+};
+
 void drawSticks()
 {
   int16_t calibStickVert = calibratedStick[CONVERT_MODE(1)];
@@ -62,7 +70,7 @@ bool menuCommonCalib(evt_t event)
   drawScreenTemplate(NULL, LBM_CALIBRATION_ICON, OPTION_MENU_NO_FOOTER);
 
   for (uint8_t i=0; i<NUM_STICKS+NUM_POTS; i++) { // get low and high vals for sticks and trims
-    int16_t vt = getAnalogValue(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<NUM_STICKS+NUM_POTS; i++) {
         reusableBuffer.calib.loVals[i] = 15000;
         reusableBuffer.calib.hiVals[i] = -15000;
-        reusableBuffer.calib.midVals[i] = getAnalogValue(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) {
+  if (calibrationState == CALIB_FINISHED) {
-    calibrationState = 0;
+    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) {
+  if (event == EVT_KEY_BREAK(KEY_EXIT) || calibrationState == CALIB_FINISHED) {
-    calibrationState = 0;
+    calibrationState = CALIB_START;
     chainMenu(menuMainView);
     return false;
   }

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<NUM_STICKS+NUM_POTS; i++) { // get low and high vals for sticks and trims
-    int16_t vt = getAnalogValue(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) {
+  if (event == EVT_KEY_BREAK(KEY_EXIT) || reusableBuffer.calib.state == CALIB_FINISHED) {
-    calibrationState = 0;
+    calibrationState = CALIB_START;
     chainMenu(menuMainView);
   }
   else {

radio/src/opentx.cpp

@@ -1487,21 +1487,40 @@ uint16_t BandGap ;
 #endif
 
 #if defined(JITTER_MEASURE)
-JitterMeter<uint16_t> rawJitter[NUMBER_ANALOG];
+  JitterMeter<uint16_t> rawJitter[NUMBER_ANALOG];
-JitterMeter<uint16_t> avgJitter[NUMBER_ANALOG];
+  JitterMeter<uint16_t> avgJitter[NUMBER_ANALOG];
-tmr10ms_t jitterResetTime = 0;
+  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<<JITTER_FILTER_STRENGTH)
+  #define ANALOG_MULTIPLIER       (1<<ANALOG_SCALE)
+  #define ANA_FILT(chan)          (s_anaFilt[chan] / (JITTER_ALPHA * ANALOG_MULTIPLIER))
+  #if (JITTER_ALPHA * ANALOG_MULTIPLIER > 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<NUMBER_ANALOG; x++) {
       rawJitter[x].reset();
       avgJitter[x].reset();
@@ -1556,41 +1576,78 @@ void getADC()
     }
   }
 
-  for (uint8_t x=0; x<NUMBER_ANALOG; x++) {
+  for (uint32_t x=0; x<NUMBER_ANALOG; x++) {
-    uint16_t v = temp[x] >> 3;
+    uint16_t v = temp[x] >> (3 - ANALOG_SCALE);
 
-#if defined(JITTER_FILTER)
+#if defined(VIRTUALINPUTS) && defined(JITTER_FILTER)
-    // jitter filter
+    // Jitter filter: 
-    uint16_t diff = (v > s_anaFilt[x]) ? (v - s_anaFilt[x]) : (s_anaFilt[x] - v);
+    //    * pass trough any big change directly
-    if (diff < 10) {
+    //    * for small change use Modified moving average (MMA) filter
-      // apply filter
+    //
-      v = (7 * s_anaFilt[x] + v) >> 3;
+    // Explanation:
-    }
+    //
-#endif
+    // Normal MMA filter has this formula:   
-
+    //            <out> = ((ALPHA-1)*<out> + <in>)/ALPHA
-#if defined(JITTER_MEASURE)
+    //
-    if (JITTER_MEASURE_ACTIVE()) {
+    // If calculation is done this way with integer arithmetics, then any small change in
-      avgJitter[x].measure(v);
+    // 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 
-#endif
+    // store undivided value between iterations. Therefore an new variable <filtered> is 
-
+    // used. The new formula becomes:
-#if defined(VIRTUALINPUTS)
+    //           <filtered> = <filtered> - <filtered>/ALPHA + <in>
-    StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[x];
+    //           <out> = <filtered>/ALPHA  (use only when out is needed)
-    if (IS_POT_MULTIPOS(x) && calib->count>0 && calib->count<XPOTS_MULTIPOS_COUNT) {
+    //
-      uint8_t vShifted = (v >> 4);
+    // The above formula with a maximum allowed ALPHA value (we are limited by 
-      s_anaFilt[x] = 2*RESX;
+    // the 16 bit s_anaFilt[]) was tested on the radio. The resulting signal still had
-      for (int i=0; i<calib->count; i++) {
+    // some jitter (a value of 1 was observed). The jitter might be bigger on other 
-        if (vShifted < calib->steps[i]) {
+    // radios.
-          s_anaFilt[x] = i*2*RESX/calib->count;
+    //
-          break;
+    // 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:
+    //   * <in> = v
+    //   * <out> = 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; i<calib->count; i++) {
+        if (vShifted < calib->steps[i]) {
+          s_anaFilt[x] = (i * ANAFILT_MAX) / calib->count;
+          break;
+        }
+      }
+    }
+#endif   // defined(VIRTUALINPUTS)
   }
 }
 #endif

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->count<XPOTS_MULTIPOS_COUNT)
 
 #if defined(PCBFLAMENCO) || defined(PCBHORUS) || (defined(PCBTARANIS) && defined(REV9E))
   #define PWR_BUTTON_DELAY

radio/src/switches.cpp

@@ -280,7 +280,7 @@ void getSwitchesPosition(bool startup)
   for (int i=0; i<NUM_XPOTS; i++) {
     if (IS_POT_MULTIPOS(POT1+i)) {
       StepsCalibData * calib = (StepsCalibData *) &g_eeGeneral.calib[POT1+i];
-      if (calib->count>0 && calib->count<XPOTS_MULTIPOS_COUNT) {
+      if (IS_MULTIPOS_CALIBRATED(calib)) {
         uint8_t pos = anaIn(POT1+i) / (2*RESX/calib->count);
         uint8_t previousPos = potsPos[i] >> 4;
         uint8_t previousStoredPos = potsPos[i] & 0x0F;