diff --git a/src/open9x.cpp b/src/open9x.cpp
index e1699de06..7c572664c 100644
--- a/src/open9x.cpp
+++ b/src/open9x.cpp
@@ -50,8 +50,8 @@ gtime_t g_unixTime; // Global date/time register, incremented each second in per
 EEGeneral  g_eeGeneral;
 ModelData  g_model;
 
-uint8_t g_tmr1Latency_max;
-uint8_t g_tmr1Latency_min;
+uint8_t g_tmr1Latency_max;
+uint8_t g_tmr1Latency_min;
 uint16_t g_timeMain;
 #ifdef DEBUG
 uint16_t g_time_per10;
@@ -318,13 +318,13 @@ int16_t  Expo::expo(int16_t x)
 #endif
 
 
-void applyExpos(int16_t *anas, uint8_t phase)
+void applyExpos(int16_t *anas, uint8_t phase)
 {
   static int16_t anas2[NUM_STICKS]; // values before expo, to ensure same expo base when multiple expo lines are used
   memcpy(anas2, anas, sizeof(anas2));
 
-  if (phase == 255)
-    phase = getFlightPhase();
+  if (phase == 255)
+    phase = getFlightPhase();
 
   int8_t cur_chn = -1;
   for (uint8_t i=0; i<MAX_EXPOS; i++) {
@@ -563,7 +563,7 @@ void setTrimValue(uint8_t phase, uint8_t idx, int16_t trim)
   STORE_MODELVARS;
 }
 
-uint8_t getTrimFlightPhase(uint8_t idx, uint8_t phase)
+uint8_t getTrimFlightPhase(uint8_t idx, uint8_t phase)
 {
   for (uint8_t i=0; i<MAX_PHASES; i++) {
     if (phase == 0) return 0;
@@ -786,7 +786,7 @@ uint8_t checkTrim(uint8_t event)
   if (k>=0 && k<8) { // && (event & _MSK_KEY_REPT))
     //LH_DWN LH_UP LV_DWN LV_UP RV_DWN RV_UP RH_DWN RH_UP
     uint8_t idx = CONVERT_MODE(1+k/2) - 1;
-    uint8_t phase = getTrimFlightPhase(idx, getFlightPhase());
+    uint8_t phase = getTrimFlightPhase(idx, getFlightPhase());
     int16_t before = getTrimValue(phase, idx);
     int8_t  v = (s==0) ? min(32, abs(before)/4+1) : 1 << (s-1); // 1=>1  2=>2  3=>4  4=>8
     bool thro = (idx==THR_STICK && g_model.thrTrim);
@@ -896,16 +896,16 @@ void getADC_filt()
 #else
 void getADC_filt()
 {
-  static uint16_t t_ana[2][8];
-  for (uint8_t adc_input=0; adc_input<8; adc_input++) {
+  static uint16_t t_ana[2][8];
+  for (uint8_t adc_input=0; adc_input<8; adc_input++) {
       ADMUX=adc_input|ADC_VREF_TYPE;
       // Start the AD conversion
       ADCSRA|=0x40;
-
-    // Do this while waiting
-    s_anaFilt[adc_input] = (s_anaFilt[adc_input]/2 + t_ana[1][adc_input]) & 0xFFFE; //gain of 2 on last conversion - clear last bit
-    t_ana[1][adc_input]  = (t_ana[1][adc_input] + t_ana[0][adc_input]) >> 1;
-
+
+    // Do this while waiting
+    s_anaFilt[adc_input] = (s_anaFilt[adc_input]/2 + t_ana[1][adc_input]) & 0xFFFE; //gain of 2 on last conversion - clear last bit
+    t_ana[1][adc_input]  = (t_ana[1][adc_input] + t_ana[0][adc_input]) >> 1;
+
       // Wait for the AD conversion to complete
       while ((ADCSRA & 0x10)==0);
       ADCSRA|=0x10;
@@ -942,20 +942,20 @@ void getADC_osmp()
 #else
 void getADC_osmp()
 {
-  uint16_t temp_ana;
-
-  for (uint8_t adc_input=0; adc_input<8; adc_input++) {
-    temp_ana = 0;
-    ADMUX = adc_input|ADC_VREF_TYPE;
+  uint16_t temp_ana;
+
+  for (uint8_t adc_input=0; adc_input<8; adc_input++) {
+    temp_ana = 0;
+    ADMUX = adc_input|ADC_VREF_TYPE;
     for (uint8_t i=0; i<4;i++) {  // Going from 10bits to 11 bits.  Addition = n.  Loop 4^n times
       // Start the AD conversion
       ADCSRA|=0x40;
       // Wait for the AD conversion to complete
       while ((ADCSRA & 0x10)==0);
       ADCSRA|=0x10;
-      temp_ana += ADCW;
+      temp_ana += ADCW;
     }
-    s_anaFilt[adc_input] = temp_ana / 2; // divide by 2^n to normalize result.
+    s_anaFilt[adc_input] = temp_ana / 2; // divide by 2^n to normalize result.
   }
 }
 #endif
@@ -975,7 +975,7 @@ void getADC_single()
 #else
 void getADC_single()
 {
-  for (uint8_t adc_input=0; adc_input<8; adc_input++) {
+  for (uint8_t adc_input=0; adc_input<8; adc_input++) {
       ADMUX=adc_input|ADC_VREF_TYPE;
       // Start the AD conversion
       ADCSRA|=0x40;
@@ -987,7 +987,7 @@ void getADC_single()
 }
 #endif
 
-#if not defined(PCBARM)
+#if not defined(PCBARM)
 void getADC_bandgap()
 {
 #if defined (PCBV4)
@@ -1110,17 +1110,17 @@ int32_t  act   [MAX_MIXERS] = {0};
 uint8_t  swOn  [MAX_MIXERS] = {0};
 uint8_t mixWarning;
 
-FORCEINLINE void evalTrims(uint8_t phase)
+FORCEINLINE void evalTrims(uint8_t phase)
 {
   for (uint8_t i=0; i<NUM_STICKS; i++) {
     // do trim -> throttle trim if applicable
     int32_t vv = 2*RESX;
     int16_t trim = getTrimValue(getTrimFlightPhase(i, phase), i);
     if (i==THR_STICK && g_model.thrTrim) {
-      if (g_eeGeneral.throttleReversed)
+      if (g_eeGeneral.throttleReversed)
         trim = -trim;
-      int16_t v = anas[i];
-      vv = ((int32_t)trim-TRIM_MIN)*(RESX-v)/(2*RESX);
+      int16_t v = anas[i];
+      vv = ((int32_t)trim-TRIM_MIN)*(RESX-v)/(2*RESX);
     }
     else if (trimsCheckTimer > 0) {
       trim = 0;
@@ -1131,7 +1131,7 @@ FORCEINLINE void evalTrims(uint8_t phase)
 }
 
 bool s_noTrainerInput = false;
-uint8_t evalSticks(uint8_t phase)
+uint8_t evalSticks(uint8_t phase)
 {
 #ifdef HELI
   uint16_t d = 0;
@@ -1162,10 +1162,10 @@ uint8_t evalSticks(uint8_t phase)
 
     if(v < -RESX) v = -RESX;
     if(v >  RESX) v =  RESX;
-
-    if (g_eeGeneral.throttleReversed && ch==THR_STICK)
-      v = -v;
-
+
+    if (g_eeGeneral.throttleReversed && ch==THR_STICK)
+      v = -v;
+
     calibratedStick[ch] = v; //for show in expo
     uint8_t tmp = (uint16_t)abs(v) / 16;
     if (tmp <= 1) anaCenter |= (tmp==0 ? 1<<ch : bpanaCenter & (1<<ch));
@@ -1196,17 +1196,17 @@ uint8_t evalSticks(uint8_t phase)
   }
 
   /* EXPOs */
-  applyExpos(anas, phase);
+  applyExpos(anas, phase);
 
   /* TRIMs */
-  evalTrims(phase);
+  evalTrims(phase);
 
   return anaCenter;
 }
 
 #ifdef DEBUG
 /*
- * This is a test function for debugging purpose, you may insert there your code and compile with the option DEBUG=YES
+ * This is a test function for debugging purpose, you may insert there your code and compile with the option DEBUG=YES
  */
 void testFunc()
 {
@@ -1273,9 +1273,9 @@ void evalFunctions()
   }
 }
 
-void perOut(uint8_t phase)
+void perOut(uint8_t phase)
 {
-  uint8_t anaCenter = evalSticks(phase);
+  uint8_t anaCenter = evalSticks(phase);
 
   //===========BEEP CENTER================
   anaCenter &= g_model.beepANACenter;
@@ -1509,11 +1509,11 @@ void perOut(uint8_t phase)
   }
 }
 
-#ifdef DISPLAY_USER_DATA
-char userDataDisplayBuf[TELEM_SCREEN_BUFFER_SIZE];
-#endif
+#ifdef DISPLAY_USER_DATA
+char userDataDisplayBuf[TELEM_SCREEN_BUFFER_SIZE];
+#endif
 
-int32_t sum_chans512[NUM_CHNOUT] = {0};
+int32_t sum_chans512[NUM_CHNOUT] = {0};
 void perMain()
 {
   static uint16_t lastTMR;
@@ -1522,48 +1522,48 @@ void perMain()
   lastTMR = tmr10ms;
 
 #define MAX_ACT 0xffff
-  static uint16_t fp_act[MAX_PHASES] = {0};
-  static uint16_t delta = 0;
-  static uint8_t s_fade_flight_phases = 0;
+  static uint16_t fp_act[MAX_PHASES] = {0};
+  static uint16_t delta = 0;
+  static uint8_t s_fade_flight_phases = 0;
   static uint8_t s_last_phase = 255;
   uint8_t phase = getFlightPhase();
   int32_t weight = 0;
 
-  if (s_last_phase != phase) {
-    if (s_last_phase == 255) {
-      fp_act[phase] = MAX_ACT;
+  if (s_last_phase != phase) {
+    if (s_last_phase == 255) {
+      fp_act[phase] = MAX_ACT;
     }
-    else {
-      uint8_t fadeTime = max(g_model.phaseData[s_last_phase].fadeOut, g_model.phaseData[phase].fadeIn);
+    else {
+      uint8_t fadeTime = max(g_model.phaseData[s_last_phase].fadeOut, g_model.phaseData[phase].fadeIn);
       if (fadeTime) {
-        s_fade_flight_phases |= (1<<s_last_phase) + (1<<phase);
+        s_fade_flight_phases |= (1<<s_last_phase) + (1<<phase);
         delta = (MAX_ACT / 100) / fadeTime;
-      }
-      else {
-        fp_act[s_last_phase] = 0;
-        fp_act[phase] = MAX_ACT;
-        s_fade_flight_phases &= ~((1<<s_last_phase) + (1<<phase));
-      }
-    }
-    s_last_phase = phase;
-  }
+      }
+      else {
+        fp_act[s_last_phase] = 0;
+        fp_act[phase] = MAX_ACT;
+        s_fade_flight_phases &= ~((1<<s_last_phase) + (1<<phase));
+      }
+    }
+    s_last_phase = phase;
+  }
 
   if (s_fade_flight_phases) {
-    memset(sum_chans512, 0, sizeof(sum_chans512));
+    memset(sum_chans512, 0, sizeof(sum_chans512));
     weight = 0;
-    for (uint8_t p=0; p<MAX_PHASES; p++) {
+    for (uint8_t p=0; p<MAX_PHASES; p++) {
       if (s_fade_flight_phases & (1<<p)) {
         perOut(p);
         for (uint8_t i=0; i<NUM_CHNOUT; i++)
           sum_chans512[i] += (chans[i] / 16) * fp_act[p];
-        weight += fp_act[p];
-      }
-    }
-    // printf("sum=%d, weight=%d ", sum_chans512[2], weight); fflush(stdout);
-    assert(weight);
+        weight += fp_act[p];
+      }
+    }
+    // printf("sum=%d, weight=%d ", sum_chans512[2], weight); fflush(stdout);
+    assert(weight);
   }
   else {
-    perOut(phase);
+    perOut(phase);
   }
 
   //========== LIMITS ===============
@@ -1616,8 +1616,8 @@ void perMain()
   uint16_t val;
 
   if (g_model.thrTraceSrc == 0) {
-    val = calibratedStick[THR_STICK]; // get throttle channel value
-    val = (g_eeGeneral.throttleReversed ? RESX-val : val+RESX);
+    val = calibratedStick[THR_STICK]+RESX; // get throttle channel value
+//    val = (g_eeGeneral.throttleReversed ? RESX-val : val+RESX);
   }
   else if (g_model.thrTraceSrc > NUM_POTS) {
     val = RESX + g_chans512[g_model.thrTraceSrc-NUM_POTS-1];
@@ -1769,27 +1769,27 @@ void perMain()
   }
   // Timers end
 
-  if (s_fade_flight_phases) {
-    for (uint8_t p=0; p<MAX_PHASES; p++) {
-      if (s_fade_flight_phases & (1<<p)) {
-        if (p == phase) {
-          if (MAX_ACT - fp_act[p] > delta)
-            fp_act[p] += delta;
-          else {
-            fp_act[p] = MAX_ACT;
-            s_fade_flight_phases -= (1<<p);
-          }
-        }
-        else {
-          if (fp_act[p] > delta)
-            fp_act[p] -= delta;
-          else {
-            fp_act[p] = 0;
-            s_fade_flight_phases -= (1<<p);
-          }
-        }
-      }
-    }
+  if (s_fade_flight_phases) {
+    for (uint8_t p=0; p<MAX_PHASES; p++) {
+      if (s_fade_flight_phases & (1<<p)) {
+        if (p == phase) {
+          if (MAX_ACT - fp_act[p] > delta)
+            fp_act[p] += delta;
+          else {
+            fp_act[p] = MAX_ACT;
+            s_fade_flight_phases -= (1<<p);
+          }
+        }
+        else {
+          if (fp_act[p] > delta)
+            fp_act[p] -= delta;
+          else {
+            fp_act[p] = 0;
+            s_fade_flight_phases -= (1<<p);
+          }
+        }
+      }
+    }
   }
   
   evalFunctions();
@@ -1813,33 +1813,33 @@ void perMain()
 #if defined (LOGS)
   writeLogs();
 #endif
-
+
 #if defined(FRSKY) && defined(DISPLAY_USER_DATA)
-  char userDataRxBuffer[21]; // Temp buffer used to collect fr-sky user data
-
-  // retrieve bytes from user data buffer and insert into display string,
-  // scrolling at the 21 character mark (edge of screen)
-  uint8_t numbytes = frskyGetUserData(userDataRxBuffer, 21); // Get as many bytes as we can
-
-  static uint8_t displayBufferIndex;
-  for (uint8_t byt=0; byt < numbytes; byt++) 
-  {
-    displayBufferIndex++;
-    if (displayBufferIndex > 20)
-    {
-      for (int xx=0; xx<20; xx++) // scroll one char left
-        userDataDisplayBuf[xx] = userDataDisplayBuf[xx+1];
-      displayBufferIndex = 20;
-    }
-    userDataDisplayBuf[displayBufferIndex] = userDataRxBuffer[byt];
-
-    // Write the raw byte out to log file, if open
-    if (testLogOpen && (g_oLogFile.fs != 0))
-      f_putc(userDataRxBuffer[byt], &g_oLogFile);
-
-  }
-#endif
-
+  char userDataRxBuffer[21]; // Temp buffer used to collect fr-sky user data
+
+  // retrieve bytes from user data buffer and insert into display string,
+  // scrolling at the 21 character mark (edge of screen)
+  uint8_t numbytes = frskyGetUserData(userDataRxBuffer, 21); // Get as many bytes as we can
+
+  static uint8_t displayBufferIndex;
+  for (uint8_t byt=0; byt < numbytes; byt++) 
+  {
+    displayBufferIndex++;
+    if (displayBufferIndex > 20)
+    {
+      for (int xx=0; xx<20; xx++) // scroll one char left
+        userDataDisplayBuf[xx] = userDataDisplayBuf[xx+1];
+      displayBufferIndex = 20;
+    }
+    userDataDisplayBuf[displayBufferIndex] = userDataRxBuffer[byt];
+
+    // Write the raw byte out to log file, if open
+    if (testLogOpen && (g_oLogFile.fs != 0))
+      f_putc(userDataRxBuffer[byt], &g_oLogFile);
+
+  }
+#endif
+
   lcd_clear();
   uint8_t evt = getEvent();
   evt = checkTrim(evt);
@@ -1853,23 +1853,23 @@ void perMain()
   g_menuStack[g_menuStackPtr](evt);
   refreshDisplay();
 
-#if defined(PCBV4)
+#if defined(PCBV4)
+  // PPM signal on phono-jack. In or out? ...
+  if(checkSlaveMode()) {
+    PORTG |= (1<<OUT_G_SIM_CTL); // 1=ppm out
+  }
+  else{
+    PORTG &=  ~(1<<OUT_G_SIM_CTL); // 0=ppm in
+  }
+#elif defined(PCBSTD)
   // PPM signal on phono-jack. In or out? ...
   if(checkSlaveMode()) {
-    PORTG |= (1<<OUT_G_SIM_CTL); // 1=ppm out
-  }
-  else{
-    PORTG &=  ~(1<<OUT_G_SIM_CTL); // 0=ppm in
-  }
-#elif defined(PCBSTD)
-  // PPM signal on phono-jack. In or out? ...
-  if(checkSlaveMode()) {
     PORTG &= ~(1<<OUT_G_SIM_CTL); // 0=ppm out
   }
   else{
     PORTG |=  (1<<OUT_G_SIM_CTL); // 1=ppm-in
   }
-#endif
+#endif
 
   switch( tmr10ms & 0x1f ) { //alle 10ms*32
 
@@ -2002,25 +2002,25 @@ ISR(TIMER0_COMP_vect, ISR_NOBLOCK) //10ms timer
 // count delta values thus can range from about 1600 to 4400 counts (800us to 2200us),
 // corresponding to a PPM signal in the range 0.8ms to 2.2ms (1.5ms at center).
 // (The timer is free-running and is thus not reset to zero at each capture interval.)
-ISR(TIMER3_CAPT_vect) // G: High frequency noise can cause stack overflo with ISR_NOBLOCK
+ISR(TIMER3_CAPT_vect) // G: High frequency noise can cause stack overflo with ISR_NOBLOCK
 {
   static uint16_t lastCapt;
 
   uint16_t capture=ICR3;
 
-  // Prevent rentrance for this IRQ only
+  // Prevent rentrance for this IRQ only
 #if defined (PCBV4)
   TIMSK3 &= ~(1<<ICIE3);
 #else
   ETIMSK &= ~(1<<TICIE3);
 #endif
-  sei(); // enable other interrupts
+  sei(); // enable other interrupts
 
   uint16_t val = (capture - lastCapt) / 2;
 
-  // G: We prcoess g_ppmIns immediately here, to make servo movement as smooth as possible
+  // G: We prcoess g_ppmIns immediately here, to make servo movement as smooth as possible
   //    while under trainee control
-  if (val>4000 && val < 16000) // G: Prioritize reset pulse. (Needed when less than 8 incoming pulses)
+  if (val>4000 && val < 16000) // G: Prioritize reset pulse. (Needed when less than 8 incoming pulses)
     ppmInState = 1; // triggered
   else
   {
@@ -2038,7 +2038,7 @@ ISR(TIMER3_CAPT_vect) // G: High frequency noise can cause stack overflo with IS
 
   lastCapt = capture;
 
-  cli(); // disable other interrupts for stack pops before this function's RETI
+  cli(); // disable other interrupts for stack pops before this function's RETI
 #if defined (PCBV4)
   TIMSK3 |= (1<<ICIE3);
 #else
@@ -2078,55 +2078,55 @@ extern uint16_t g_timeMain;
 
 #endif
 
-/*
-   USART0 Transmit Data Register Emtpy ISR
-   Used to transmit FrSky data packets and DSM2 protocol
-*/
-
-#if defined (FRSKY)
-FORCEINLINE void FRSKY_USART0_vect()
-{
-  if (frskyTxBufferCount > 0) {
-    UDR0 = frskyTxBuffer[--frskyTxBufferCount];
-  }
-  else {
-    UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
-  }
-}
-#endif
-
-#if defined (DSM2_SERIAL)
-FORCEINLINE void DSM2_USART0_vect()
+/*
+   USART0 Transmit Data Register Emtpy ISR
+   Used to transmit FrSky data packets and DSM2 protocol
+*/
+
+#if defined (FRSKY)
+FORCEINLINE void FRSKY_USART0_vect()
 {
-  UDR0 = *((uint16_t*)pulses2MHzRPtr);
+  if (frskyTxBufferCount > 0) {
+    UDR0 = frskyTxBuffer[--frskyTxBufferCount];
+  }
+  else {
+    UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
+  }
+}
+#endif
+
+#if defined (DSM2_SERIAL)
+FORCEINLINE void DSM2_USART0_vect()
+{
+  UDR0 = *((uint16_t*)pulses2MHzRPtr);
+
+  pulses2MHzRPtr += sizeof(uint16_t);
+  if (pulses2MHzRPtr == pulses2MHzWPtr) {
+    UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
+  }
+}
+#endif
+
+#ifndef SIMU
+#if defined (FRSKY) or defined(DSM2_SERIAL)
+ISR(USART0_UDRE_vect)
+{
+#if defined (FRSKY) and defined (DSM2_SERIAL)
+  if (g_model.protocol == PROTO_DSM2) {
+    DSM2_USART0_vect();
+  }
+  else {
+    FRSKY_USART0_vect();
+  }
+#elif defined (FRSKY)
+  FRSKY_USART0_vect();
+#else
+  DSM2_USART0_vect();
+#endif
+}
+#endif
+#endif
 
-  pulses2MHzRPtr += sizeof(uint16_t);
-  if (pulses2MHzRPtr == pulses2MHzWPtr) {
-    UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
-  }
-}
-#endif
-
-#ifndef SIMU
-#if defined (FRSKY) or defined(DSM2_SERIAL)
-ISR(USART0_UDRE_vect)
-{
-#if defined (FRSKY) and defined (DSM2_SERIAL)
-  if (g_model.protocol == PROTO_DSM2) {
-    DSM2_USART0_vect();
-  }
-  else {
-    FRSKY_USART0_vect();
-  }
-#elif defined (FRSKY)
-  FRSKY_USART0_vect();
-#else
-  DSM2_USART0_vect();
-#endif
-}
-#endif
-#endif
-
 #if defined (PCBV4)
 /*---------------------------------------------------------*/
 /* User Provided Date/Time Function for FatFs module       */
@@ -2153,14 +2153,14 @@ uint32_t o9x_get_fattime(void) // TODO why not in ff.cpp?
 
 void instantTrim()
 {
-  uint8_t phase = getFlightPhase();
-
+  uint8_t phase = getFlightPhase();
+
   for (uint8_t i=0; i<NUM_STICKS; i++) {
     if (i!=THR_STICK) {
       // don't instant trim the throttle stick
-      uint8_t trim_phase = getTrimFlightPhase(i, phase);
+      uint8_t trim_phase = getTrimFlightPhase(i, phase);
       s_noTrainerInput = true;
-      evalSticks(phase);
+      evalSticks(phase);
       s_noTrainerInput = false;
       int16_t trim = (anas[i] + trims[i]) / 2;
       if (trim < TRIM_EXTENDED_MIN) {
@@ -2177,37 +2177,37 @@ void instantTrim()
   AUDIO_WARNING2();
 }
 
-#if defined (PCBV4)
-// Rotary encoder interrupts
+#if defined (PCBV4)
+// Rotary encoder interrupts
 volatile uint8_t g_rotenc[2] = {0};
 #endif
 
-#ifndef SIMU
+#ifndef SIMU
 
 #if defined (PCBV4)
-ISR(INT2_vect)
-{
-  uint8_t input = PIND & 0b00001100;
-  if (input == 0 || input == 0b00001100) g_rotenc[0]--;
-}
-ISR(INT3_vect)
-{
-  uint8_t input = PIND & 0b00001100;
-  if (input == 0 || input == 0b00001100) g_rotenc[0]++;
-}
-
-ISR(INT5_vect)
-{
-  uint8_t input = PINE & 0b01100000;
-  if (input == 0 || input == 0b01100000) g_rotenc[1]++;
-}
-ISR(INT6_vect)
-{
-  uint8_t input = PINE & 0b01100000;
-  if (input == 0 || input == 0b01100000) g_rotenc[1]--;
-}
+ISR(INT2_vect)
+{
+  uint8_t input = PIND & 0b00001100;
+  if (input == 0 || input == 0b00001100) g_rotenc[0]--;
+}
+ISR(INT3_vect)
+{
+  uint8_t input = PIND & 0b00001100;
+  if (input == 0 || input == 0b00001100) g_rotenc[0]++;
+}
+
+ISR(INT5_vect)
+{
+  uint8_t input = PINE & 0b01100000;
+  if (input == 0 || input == 0b01100000) g_rotenc[1]++;
+}
+ISR(INT6_vect)
+{
+  uint8_t input = PINE & 0b01100000;
+  if (input == 0 || input == 0b01100000) g_rotenc[1]--;
+}
 #endif
-
+
 extern unsigned char __bss_end ;
 
 uint16_t stack_free()
@@ -2250,16 +2250,16 @@ int main(void)
 
   lcdSetRefVolt(25);
 
-  sei(); // interrupts needed for FRSKY_Init and eeReadAll.
+  sei(); // interrupts needed for FRSKY_Init and eeReadAll.
 
 #if defined (FRSKY) and !defined (DSM2_SERIAL)
   FRSKY_Init();
 #endif
 
-#if defined (DSM2_SERIAL) and !defined (FRSKY)
-  DSM2_Init();
-#endif
-
+#if defined (DSM2_SERIAL) and !defined (FRSKY)
+  DSM2_Init();
+#endif
+
 #ifdef JETI
   JETI_Init();
 #endif
@@ -2304,7 +2304,7 @@ int main(void)
     checkAlarm();
   }
 
-  clearKeyEvents(); //make sure no keys are down before proceeding
+  clearKeyEvents(); //make sure no keys are down before proceeding
 
   lcdSetRefVolt(g_eeGeneral.contrast);
   g_LightOffCounter = g_eeGeneral.lightAutoOff*500; //turn on light for x seconds - no need to press key Issue 152
@@ -2346,7 +2346,7 @@ int main(void)
   /***************************************************/
 
   /*
-   * SOMO set-up (V4 board only)
+   * SOMO set-up (V4 board only)
    */
   OCR4A = 0x7d;
   TCCR4B = (1 << WGM42) | (3<<CS40); // CTC OCR1A, 16MHz / 64 (4us ticks)