diff --git a/src/Makefile b/src/Makefile
index 628eb6a68..61ff5b4c9 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -60,7 +60,7 @@ PCBREV =
 # Enable JETI-Telemetry or FrSky Telemetry reception on UART0
 # For this option you need to modify your hardware!
 # More information at [insertURLhere]
-# Values = STD, FRSKY, JETI, NMEA, ARDUPILOT, MAVLINK
+# Values = STD, FRSKY, JETI, NMEA, ARDUPILOT, MAVLINK, TELEMETREZ
 EXT = STD
 
 # Enable heli menu
@@ -103,10 +103,6 @@ HAPTIC = NO
 # Values = YES, NO 
 SP22 = NO
 
-# TelemetrEZ
-# Values = YES, NO 
-TELEMETREZ = NO
-
 # SPLASH on START
 # Values = YES, NO, FIXED
 SPLASH = YES
@@ -437,8 +433,12 @@ ifeq ($(EXT), MAVLINK)
  CPPSRC += mavlink.cpp rotarysw.cpp serial.cpp
 endif
 
+ifeq ($(EXT), TELEMETREZ)
+  CPPDEFS += -DTELEMETREZ
+endif
+
 # If FRSKY-Support is enabled
-ifeq ($(EXT), FRSKY)
+ifeq ($(EXT), $(filter $(EXT), FRSKY TELEMETREZ))
   CPPDEFS += -DFRSKY
   CPPSRC += frsky.cpp
   # If FRSKY-Hub is enabled
@@ -627,10 +627,6 @@ ifeq ($(PCB), STD128)
   ifeq ($(SP22), YES)
    CPPDEFS += -DSP22
   endif
-  
-  ifeq ($(TELEMETREZ), YES)
-   CPPDEFS += -DTELEMETREZ
-  endif
 endif
 
 ifeq ($(PCB), STD)
@@ -663,10 +659,6 @@ ifeq ($(PCB), STD)
   ifeq ($(SP22), YES)
    CPPDEFS += -DSP22
   endif
-  
-  ifeq ($(TELEMETREZ), YES)
-   CPPDEFS += -DTELEMETREZ
-  endif
 endif
 
 ### Global Build-Option Directives ###
diff --git a/src/gruvin9x/board_gruvin9x.h b/src/gruvin9x/board_gruvin9x.h
index 57cbc8970..9077c71e1 100644
--- a/src/gruvin9x/board_gruvin9x.h
+++ b/src/gruvin9x/board_gruvin9x.h
@@ -95,4 +95,78 @@ void sdPoll10ms();
 #define __BACKLIGHT_OFF PORTC &= ~(1 << OUT_C_LIGHT)
 #define IS_BACKLIGHT_ON() (PORTC & (1<<OUT_C_LIGHT))
 
+
+#  define INP_L_SPARE6    7
+#  define INP_L_SPARE5    6
+#  define INP_L_KEY_EXT   5
+#  define INP_L_KEY_MEN   4
+#  define INP_L_KEY_LFT   3
+#  define INP_L_KEY_RGT   2
+#  define INP_L_KEY_UP    1
+#  define INP_L_KEY_DWN   0
+
+#  define INP_J_TRM_RH_UP   7
+#  define INP_J_TRM_RH_DWN  6
+#  define INP_J_TRM_RV_UP   5
+#  define INP_J_TRM_RV_DWN  4
+#  define INP_J_TRM_LV_UP   3
+#  define INP_J_TRM_LV_DWN  2
+#  define INP_J_TRM_LH_UP   1
+#  define INP_J_TRM_LH_DWN  0
+
+#  define INP_E_PPM_IN      7
+#  define INP_E_ROT_ENC_1_B 6
+#  define INP_E_ROT_ENC_1_A 5
+#  define INP_E_USB_D_PLS   4
+#  define OUT_E_BUZZER      3
+#  define INP_E_USB_D_NEG   2
+#  define INP_E_TELEM_RX    1
+#  define OUT_E_TELEM_TX    0
+
+#  define OUT_D_HAPTIC         7
+#  define INP_D_SPARE4         6
+#  define INP_D_ROT_ENC_2_PUSH 5
+#  define INP_D_ROT_ENC_1_PUSH 4
+#  define OUT_D_ROT_ENC_2_B    3
+#  define INP_D_ROT_ENC_2_A    2
+#  define INP_D_I2C_SCL        1
+#  define INP_D_I2C_SDA        0
+
+#  define INP_G_Gear     5
+#  define INP_G_ThrCt    2
+#  define OUT_G_SIM_CTL  4 //1 : phone-jack=ppm_in
+#  define INP_G_ID1      3
+#  define INP_G_RF_POW   1
+#  define INP_G_RuddDR   0
+
+#  define INP_C_AileDR   7
+#  define INP_C_ElevDR   6
+#  define OUT_C_LIGHT    0
+
+#  define OUT_B_Speaker  7
+#  define OUT_B_PPM      6 // will be switched by TCNT1==OCR1B in hardware
+#  define INP_B_Trainer  5
+#  define INP_B_ID2      4
+
+#if defined(VOICE)
+#  define OUT_H_14DRESET 3
+#  define OUT_H_14DCLK   4
+#  define OUT_H_14DDATA  5
+#  define INP_H_14DBUSY  6
+#endif
+
+#define KEYS_PRESSED() (~PINL) // used only for DBLKEYS code.
+#define DBLKEYS_PRESSED_RGT_LFT(i) ((in & ((1<<INP_L_KEY_RGT) + (1<<INP_L_KEY_LFT))) == ((1<<INP_L_KEY_RGT) + (1<<INP_L_KEY_LFT)))
+#define DBLKEYS_PRESSED_UP_DWN(i)  ((in & ((1<<INP_L_KEY_UP)  + (1<<INP_L_KEY_DWN))) == ((1<<INP_L_KEY_UP)  + (1<<INP_L_KEY_DWN)))
+#define DBLKEYS_PRESSED_RGT_UP(i)  ((in & ((1<<INP_L_KEY_RGT) + (1<<INP_L_KEY_UP)))  == ((1<<INP_L_KEY_RGT) + (1<<INP_L_KEY_UP)))
+#define DBLKEYS_PRESSED_LFT_DWN(i) ((in & ((1<<INP_L_KEY_LFT) + (1<<INP_L_KEY_DWN))) == ((1<<INP_L_KEY_LFT) + (1<<INP_L_KEY_DWN)))
+
+#define PORTA_LCD_DAT  PORTA
+#define PORTC_LCD_CTRL PORTC
+#define OUT_C_LCD_E     5
+#define OUT_C_LCD_RnW   4
+#define OUT_C_LCD_A0    3
+#define OUT_C_LCD_RES   2
+#define OUT_C_LCD_CS1   1
+
 #endif
diff --git a/src/open9x.cpp b/src/open9x.cpp
index a891f713d..653ff568d 100644
--- a/src/open9x.cpp
+++ b/src/open9x.cpp
@@ -873,6 +873,9 @@ bool __getSwitch(int8_t swtch)
     if (cs->duration) {
       if (result && !cswStates[cs_idx])
         cswDurations[cs_idx] = get_tmr10ms() + (cs->duration*50);
+
+      cswStates[cs_idx] = result;
+
       if (cswDurations[cs_idx] > get_tmr10ms()) {
         result = true;
         if (cs->delay) cswDelays[cs_idx] = get_tmr10ms() + (cs->delay*50);
diff --git a/src/open9x.h b/src/open9x.h
index 8ec59ec19..48ae8ca16 100644
--- a/src/open9x.h
+++ b/src/open9x.h
@@ -322,172 +322,6 @@ extern uint8_t s_bind_allowed;
 #endif
 #endif
 
-// G: The following comments relate to the original stock PCB only
-//
-//                  elev                        thr
-//                   LV                         RV
-//                 2 ^                        4 ^
-//                   1                          2
-//                   |     rudd                 |     aile
-//              <----X--3-> LH             <----X--0-> RH
-//              6    |    7                1    |    0
-//                   |                          |
-//                 3 v                        5 v
-//
-//PORTA  7      6       5       4       3       2       1       0
-//       O      O       O       O       O       O       O       O
-//       ------------------------ LCD_DAT -----------------------
-//
-//PORTB  7      6       5       4       3       2       1       0
-//       O      i       i       i       i       i       i       O
-//       light  KEY_LFT KEY_RGT KEY_UP  KEY_DWN KEY_EXT KEY_MEN  PPM
-//
-//PORTC  7      6       5       4       3       2       1       0
-//       -      -       O       O       O       O       O       -
-//       NC     NC    LCD_E   LCD_RNW  LCD_A0  LCD_RES LCD_CS1  NC
-//
-//PORTD  7      6       5       4       3       2       1       0
-//       i      i       i       i       i       i       i       i
-//     TRM_D_DWN _UP  TRM_C_DWN _UP   TRM_B_DWN _UP   TRM_A_DWN _UP
-//
-//PORTE  7      6       5       4       3       2       1       0
-//       i      i       i       i       O       i       i       i
-//     PPM_IN  ID2    Trainer  Gear   Buzzer   ElevDR  AileDR  THRCT
-//
-//PORTF  7      6       5       4       3       2       1       0
-//       ai     ai      ai      ai      ai      ai      ai      ai
-// ANA_ BAT   PITT_TRM HOV_THR HOV_PIT  STCK_LH STCK_RV STCK_LV STCK_RH
-//                                      rud    thro   elev   aile
-//PORTG  7      6       5       4       3       2       1       0
-//       -      -       -       O       i               i       i
-//                            SIM_CTL  ID1      Haptic      RF_POW RuddDR
-
-#define PORTA_LCD_DAT  PORTA
-#define PORTC_LCD_CTRL PORTC
-#define OUT_C_LCD_E     5
-#define OUT_C_LCD_RnW   4
-#define OUT_C_LCD_A0    3
-#define OUT_C_LCD_RES   2
-#define OUT_C_LCD_CS1   1
-
-#if defined (PCBGRUVIN9X)
-
-#  define INP_L_SPARE6    7
-#  define INP_L_SPARE5    6
-#  define INP_L_KEY_EXT   5
-#  define INP_L_KEY_MEN   4
-#  define INP_L_KEY_LFT   3
-#  define INP_L_KEY_RGT   2
-#  define INP_L_KEY_UP    1
-#  define INP_L_KEY_DWN   0
-
-#  define INP_J_TRM_RH_UP   7
-#  define INP_J_TRM_RH_DWN  6
-#  define INP_J_TRM_RV_UP   5
-#  define INP_J_TRM_RV_DWN  4
-#  define INP_J_TRM_LV_UP   3
-#  define INP_J_TRM_LV_DWN  2
-#  define INP_J_TRM_LH_UP   1
-#  define INP_J_TRM_LH_DWN  0
-
-#  define INP_E_PPM_IN      7
-#  define INP_E_ROT_ENC_1_B 6
-#  define INP_E_ROT_ENC_1_A 5
-#  define INP_E_USB_D_PLS   4
-#  define OUT_E_BUZZER      3
-#  define INP_E_USB_D_NEG   2
-#  define INP_E_TELEM_RX    1
-#  define OUT_E_TELEM_TX    0
-
-#  define OUT_D_HAPTIC         7
-#  define INP_D_SPARE4         6
-#  define INP_D_ROT_ENC_2_PUSH 5
-#  define INP_D_ROT_ENC_1_PUSH 4
-#  define OUT_D_ROT_ENC_2_B    3
-#  define INP_D_ROT_ENC_2_A    2
-#  define INP_D_I2C_SCL        1
-#  define INP_D_I2C_SDA        0
-
-#  define INP_G_Gear     5
-#  define INP_G_ThrCt    2
-#  define OUT_G_SIM_CTL  4 //1 : phone-jack=ppm_in
-#  define INP_G_ID1      3
-#  define INP_G_RF_POW   1
-#  define INP_G_RuddDR   0
-
-#  define INP_C_AileDR   7
-#  define INP_C_ElevDR   6
-#  define OUT_C_LIGHT    0
-
-#  define OUT_B_Speaker  7
-#  define OUT_B_PPM      6 // will be switched by TCNT1==OCR1B in hardware
-#  define INP_B_Trainer  5
-#  define INP_B_ID2      4
-
-#if defined(VOICE)
-#  define OUT_H_14DRESET 3
-#  define OUT_H_14DCLK   4
-#  define OUT_H_14DDATA  5
-#  define INP_H_14DBUSY  6
-#endif
-
-#define KEYS_PRESSED() (~PINL) // used only for DBLKEYS code.
-#define DBLKEYS_PRESSED_RGT_LFT(i) ((in & ((1<<INP_L_KEY_RGT) + (1<<INP_L_KEY_LFT))) == ((1<<INP_L_KEY_RGT) + (1<<INP_L_KEY_LFT)))
-#define DBLKEYS_PRESSED_UP_DWN(i)  ((in & ((1<<INP_L_KEY_UP)  + (1<<INP_L_KEY_DWN))) == ((1<<INP_L_KEY_UP)  + (1<<INP_L_KEY_DWN)))
-#define DBLKEYS_PRESSED_RGT_UP(i)  ((in & ((1<<INP_L_KEY_RGT) + (1<<INP_L_KEY_UP)))  == ((1<<INP_L_KEY_RGT) + (1<<INP_L_KEY_UP)))
-#define DBLKEYS_PRESSED_LFT_DWN(i) ((in & ((1<<INP_L_KEY_LFT) + (1<<INP_L_KEY_DWN))) == ((1<<INP_L_KEY_LFT) + (1<<INP_L_KEY_DWN)))
-
-
-#elif defined(PCBSTD) // stock board ...
-
-#define OUT_B_LIGHT   7
-#define INP_B_KEY_LFT 6
-#define INP_B_KEY_RGT 5
-#define INP_B_KEY_UP  4
-#define INP_B_KEY_DWN 3
-#define INP_B_KEY_EXT 2
-#define INP_B_KEY_MEN 1
-#define OUT_B_PPM     0
-
-#define INP_D_TRM_LH_UP   7
-#define INP_D_TRM_LH_DWN  6
-#define INP_D_TRM_RV_DWN  5
-#define INP_D_TRM_RV_UP   4
-#define INP_D_TRM_LV_DWN  3
-#define INP_D_TRM_LV_UP   2
-#define INP_D_TRM_RH_DWN  1
-#define INP_D_TRM_RH_UP   0
-
-#define INP_E_PPM_IN  7
-#define INP_E_ID2     6
-#define INP_E_Trainer 5
-#define INP_E_Gear    4
-#define OUT_E_BUZZER  3
-#define INP_E_ElevDR  2
-#define INP_E_AileDR  1
-#define INP_E_ThrCt   0
-
-#if defined(JETI) || defined(FRSKY) || defined(ARDUPILOT) || defined(NMEA) || defined(MAVLINK)
-#undef INP_E_ThrCt
-#undef INP_E_AileDR
-#define INP_C_ThrCt   6
-#define INP_C_AileDR  7
-#endif
-
-#define OUT_G_SIM_CTL  4 //1 : phone-jack=ppm_in
-#define INP_G_ID1      3
-#define OUT_G_HAPTIC   2
-#define INP_G_RF_POW   1
-#define INP_G_RuddDR   0
-
-#define KEYS_PRESSED() (~PINB)
-#define DBLKEYS_PRESSED_RGT_LFT(i) ((in & ((1<<INP_B_KEY_RGT) + (1<<INP_B_KEY_LFT))) == ((1<<INP_B_KEY_RGT) + (1<<INP_B_KEY_LFT)))
-#define DBLKEYS_PRESSED_UP_DWN(i)  ((in & ((1<<INP_B_KEY_UP)  + (1<<INP_B_KEY_DWN))) == ((1<<INP_B_KEY_UP)  + (1<<INP_B_KEY_DWN)))
-#define DBLKEYS_PRESSED_RGT_UP(i)  ((in & ((1<<INP_B_KEY_RGT) + (1<<INP_B_KEY_UP)))  == ((1<<INP_B_KEY_RGT) + (1<<INP_B_KEY_UP)))
-#define DBLKEYS_PRESSED_LFT_DWN(i) ((in & ((1<<INP_B_KEY_LFT) + (1<<INP_B_KEY_DWN))) == ((1<<INP_B_KEY_LFT) + (1<<INP_B_KEY_DWN)))
-
-#endif // defined (PCBGRUVIN9X)
-
 extern const pm_uint8_t bchout_ar[];
 extern const pm_uint8_t modn12x3[];
 
diff --git a/src/stock/board_stock.h b/src/stock/board_stock.h
index 9b3d46091..b195f71d3 100644
--- a/src/stock/board_stock.h
+++ b/src/stock/board_stock.h
@@ -93,4 +93,97 @@ bool checkSlaveMode();
 #define IS_BACKLIGHT_ON() (PORTB & (1<<OUT_B_LIGHT))
 #endif
 
+// G: The following comments relate to the original stock PCB only
+//
+//                  elev                        thr
+//                   LV                         RV
+//                 2 ^                        4 ^
+//                   1                          2
+//                   |     rudd                 |     aile
+//              <----X--3-> LH             <----X--0-> RH
+//              6    |    7                1    |    0
+//                   |                          |
+//                 3 v                        5 v
+//
+//PORTA  7      6       5       4       3       2       1       0
+//       O      O       O       O       O       O       O       O
+//       ------------------------ LCD_DAT -----------------------
+//
+//PORTB  7      6       5       4       3       2       1       0
+//       O      i       i       i       i       i       i       O
+//       light  KEY_LFT KEY_RGT KEY_UP  KEY_DWN KEY_EXT KEY_MEN  PPM
+//
+//PORTC  7      6       5       4       3       2       1       0
+//       -      -       O       O       O       O       O       -
+//       NC     NC    LCD_E   LCD_RNW  LCD_A0  LCD_RES LCD_CS1  NC
+//
+//PORTD  7      6       5       4       3       2       1       0
+//       i      i       i       i       i       i       i       i
+//     TRM_D_DWN _UP  TRM_C_DWN _UP   TRM_B_DWN _UP   TRM_A_DWN _UP
+//
+//PORTE  7      6       5       4       3       2       1       0
+//       i      i       i       i       O       i       i       i
+//     PPM_IN  ID2    Trainer  Gear   Buzzer   ElevDR  AileDR  THRCT
+//
+//PORTF  7      6       5       4       3       2       1       0
+//       ai     ai      ai      ai      ai      ai      ai      ai
+// ANA_ BAT   PITT_TRM HOV_THR HOV_PIT  STCK_LH STCK_RV STCK_LV STCK_RH
+//                                      rud    thro   elev   aile
+//PORTG  7      6       5       4       3       2       1       0
+//       -      -       -       O       i               i       i
+//                            SIM_CTL  ID1      Haptic      RF_POW RuddDR
+
+#define OUT_B_LIGHT   7
+#define INP_B_KEY_LFT 6
+#define INP_B_KEY_RGT 5
+#define INP_B_KEY_UP  4
+#define INP_B_KEY_DWN 3
+#define INP_B_KEY_EXT 2
+#define INP_B_KEY_MEN 1
+#define OUT_B_PPM     0
+
+#define INP_D_TRM_LH_UP   7
+#define INP_D_TRM_LH_DWN  6
+#define INP_D_TRM_RV_DWN  5
+#define INP_D_TRM_RV_UP   4
+#define INP_D_TRM_LV_DWN  3
+#define INP_D_TRM_LV_UP   2
+#define INP_D_TRM_RH_DWN  1
+#define INP_D_TRM_RH_UP   0
+
+#define INP_E_PPM_IN  7
+#define INP_E_ID2     6
+#define INP_E_Trainer 5
+#define INP_E_Gear    4
+#define OUT_E_BUZZER  3
+#define INP_E_ElevDR  2
+
+#if defined(JETI) || defined(FRSKY) || defined(ARDUPILOT) || defined(NMEA) || defined(MAVLINK)
+#define INP_C_ThrCt   6
+#define INP_C_AileDR  7
+#else
+#define INP_E_ThrCt   0
+#define INP_E_AileDR  1
+#endif
+
+#define OUT_G_SIM_CTL  4 //1 : phone-jack=ppm_in
+#define INP_G_ID1      3
+#define OUT_G_HAPTIC   2
+#define INP_G_RF_POW   1
+#define INP_G_RuddDR   0
+
+#define KEYS_PRESSED() (~PINB)
+#define DBLKEYS_PRESSED_RGT_LFT(i) ((in & ((1<<INP_B_KEY_RGT) + (1<<INP_B_KEY_LFT))) == ((1<<INP_B_KEY_RGT) + (1<<INP_B_KEY_LFT)))
+#define DBLKEYS_PRESSED_UP_DWN(i)  ((in & ((1<<INP_B_KEY_UP)  + (1<<INP_B_KEY_DWN))) == ((1<<INP_B_KEY_UP)  + (1<<INP_B_KEY_DWN)))
+#define DBLKEYS_PRESSED_RGT_UP(i)  ((in & ((1<<INP_B_KEY_RGT) + (1<<INP_B_KEY_UP)))  == ((1<<INP_B_KEY_RGT) + (1<<INP_B_KEY_UP)))
+#define DBLKEYS_PRESSED_LFT_DWN(i) ((in & ((1<<INP_B_KEY_LFT) + (1<<INP_B_KEY_DWN))) == ((1<<INP_B_KEY_LFT) + (1<<INP_B_KEY_DWN)))
+
+#define PORTA_LCD_DAT  PORTA
+#define PORTC_LCD_CTRL PORTC
+#define OUT_C_LCD_E     5
+#define OUT_C_LCD_RnW   4
+#define OUT_C_LCD_A0    3
+#define OUT_C_LCD_RES   2
+#define OUT_C_LCD_CS1   1
+
 #endif