diff --git a/BootLoaders/Boards/platform.local.txt b/BootLoaders/Boards/platform.local.txt
index b7db254..1322d9b 100644
--- a/BootLoaders/Boards/platform.local.txt
+++ b/BootLoaders/Boards/platform.local.txt
@@ -1 +1,3 @@
-# Empty
+## Save hex
+recipe.output.tmp_file={build.project_name}.hex
+recipe.output.save_file=multifw.hex
diff --git a/BootLoaders/Boards/platform.txt b/BootLoaders/Boards/platform.txt
index e41c5cc..8dd0c1d 100644
--- a/BootLoaders/Boards/platform.txt
+++ b/BootLoaders/Boards/platform.txt
@@ -1,9 +1,7 @@
-
-# Arduino AVR Core and platform.
-# ------------------------------
-
+#
+#
 # For more info:
 # https://github.com/arduino/Arduino/wiki/Arduino-IDE-1.5---3rd-party-Hardware-specification
 
-name=Multi 4-in-1 Boards
+name=Multi 4-in-1 AVR
 version=1.0.0
diff --git a/BootLoaders/package_multi_4in1_board_index.json b/BootLoaders/package_multi_4in1_board_index.json
index 911e9d5..72cf2fb 100644
--- a/BootLoaders/package_multi_4in1_board_index.json
+++ b/BootLoaders/package_multi_4in1_board_index.json
@@ -10,7 +10,7 @@
       },
       "platforms": [
         {
-          "name": "Multi 4-in-1 Boards",
+          "name": "Multi 4-in-1 AVR Board",
           "architecture": "avr",
           "version": "1.0",
           "category": "Contributed",
@@ -19,8 +19,8 @@
           },
           "url": "https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/raw/master/BootLoaders/package_multi_4in1_board_v1.0.0.zip",
           "archiveFileName": "package_multi_4in1_board_v1.0.0.zip",
-          "checksum": "SHA-256:61AB463B5B91BCCBF285EB4C801B37E4F46138CA8762E4C04CCB09C2FF157CB7",
-          "size": "3161",
+          "checksum": "SHA-256:18388AD5C6FDBA45474D1B69D7521A5AFBCF5A2790D9EB0D91E07508BC2B40F9",
+          "size": "3201",
           "boards": [
             {"name": "Multi 4-in-1 (Atmega328p, 3.3V, 16MHz)"}
           ],
diff --git a/BootLoaders/package_multi_4in1_board_v1.0.0.zip b/BootLoaders/package_multi_4in1_board_v1.0.0.zip
index b0f14fd..40818ee 100644
Binary files a/BootLoaders/package_multi_4in1_board_v1.0.0.zip and b/BootLoaders/package_multi_4in1_board_v1.0.0.zip differ
diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino
index e37e551..8f536af 100644
--- a/Multiprotocol/Devo_cyrf6936.ino
+++ b/Multiprotocol/Devo_cyrf6936.ino
@@ -63,7 +63,7 @@ static void __attribute__((unused)) DEVO_add_pkt_suffix()
 		BIND_SET_PULLUP;										// set pullup
 		if(IS_BIND_BUTTON_on)
 		{
-			eeprom_write_byte((EE_ADDR)(30+mode_select),0x01);	// Set fixed id mode for the current model
+			eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x01);	// Set fixed id mode for the current model
 			option=1;
 		}
 		BIND_SET_OUTPUT;
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 0f0e84b..b06cc13 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -19,7 +19,7 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		1
 #define VERSION_REVISION	6
-#define VERSION_PATCH_LEVEL	29
+#define VERSION_PATCH_LEVEL	30
 //******************
 // Protocols
 //******************
@@ -224,14 +224,29 @@ struct PPM_Parameters
 // Telemetry
 
 enum MultiPacketTypes {
-    MULTI_TELEMETRY_STATUS  = 1,
-    MULTI_TELEMETRY_SPORT   = 2,
-    MULTI_TELEMETRY_HUB     = 3,
-    MULTI_TELEMETRY_DSM     = 4,
-    MULTI_TELEMETRY_DSMBIND = 5,
-    MULTI_TELEMETRY_AFHDS2A = 6,
+	MULTI_TELEMETRY_STATUS  = 1,
+	MULTI_TELEMETRY_SPORT   = 2,
+	MULTI_TELEMETRY_HUB     = 3,
+	MULTI_TELEMETRY_DSM     = 4,
+	MULTI_TELEMETRY_DSMBIND = 5,
+	MULTI_TELEMETRY_AFHDS2A = 6,
+	MULTI_TELEMETRY_INPUTSYNC=8,
+	MULTI_COMMAND_CONFIG    = 0x80,
+	MULTI_COMMAND_FAILSAFE	 =0x81,
 };
 
+enum FailSafeMode {
+	FAILSAFE_NOTSET 	= 0,
+	FAILSAFE_HOLD		= 1,
+	FAILSAFE_CUSTOM		= 2,
+	FAILSAFE_NOPULSES	= 3,
+	FAILSAFE_RECEIVER	= 4,
+	// Use during update so we can get away with only one copy of Failsafe channels
+	FAILSEFASE_INVALID	= 0xfe
+};
+
+#define FAILSAFE_CHANNEL_HOLD	0
+#define	FAILSAFE_CHANNEL_NOPULSES	2047
 
 // Macros
 #define NOP() __asm__ __volatile__("nop")
@@ -306,6 +321,23 @@ enum MultiPacketTypes {
 #define IS_WAIT_BIND_on		( ( protocol_flags2 & _BV(7) ) !=0 )
 #define IS_WAIT_BIND_off	( ( protocol_flags2 & _BV(7) ) ==0 )
 
+//Configuration
+#define IS_TELEMTRY_INVERSION_ON	(multi_config & 0x01)
+#define IS_MULTI_TELEMETRY_ON		(multi_config & 0x02)
+#define IS_EXTRA_TELEMETRY_ON       (multi_config & 0x04)
+
+// Failsafe
+#define failsafeToPPM(i)			(Failsafe_data[i]* 5/8+860)
+#define isNormalFailsafeChanel(i)	(Failsafe_data[i] != FAILSAFE_CHANNEL_HOLD && Failsafe_data[i] != FAILSAFE_CHANNEL_NOPULSES)
+
+
+//Status messages
+#if defined(STM32_BOARD) && defined (SERIAL_DEBUG)
+	#define debug(msg, ...)  {char buf[64]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); for(int i=0;buf[i] !=0; i++) StatusSerial_write(buf[i]);}
+#else
+	#define debug(...)
+	#undef SERIAL_DEBUG
+#endif
 
 //********************
 //*** Blink timing ***
@@ -443,6 +475,14 @@ enum {
 #define SPEED_57600	2
 #define SPEED_125K	3
 
+/** EEPROM Layout */
+#define EEPROM_ID_OFFSET		10		// Module ID (4 bytes)
+#define EEPROM_ID_VALID_OFFSET	20		// 1 byte flag that ID is valid
+#define MODELMODE_EEPROM_OFFSET	30		// Autobind mode, 1 byte per model, end is 46
+#define AFHDS2A_EEPROM_OFFSET	50		// RX ID, 4 byte per model id, end is 114
+#define CONFIG_EEPROM_OFFSET 	120		// Current configuration of the multimodule
+
+
 //****************************************
 //*** MULTI protocol serial definition ***
 //****************************************
@@ -637,7 +677,7 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
    version of multi code, should be displayed as major.minor.revision.patchlevel
 */
 /*
-  Multiprotocol telemetry definition for OpenTX
+  Multiprotocol telemetry/command definition for OpenTX
   Based on #define MULTI_TELEMETRY enables OpenTX to get the multimodule status and select the correct telemetry type automatically.
 
   Serial: 100000 Baud 8e2 (same as input)
@@ -657,6 +697,8 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
 
    [4-xx] data
 
+  Commands from TX to multi cannot be longer than 22 bytes (RXLen -4byte header)
+
   Type = 0x01 Multimodule Status:
    [4] Flags
    0x01 = Input signal detected
@@ -686,12 +728,53 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
   Type 0x05 DSM bind data
 	data[0-16] DSM bind data
 
-    technically DSM bind data is only 10 bytes but multi send 16
-    like with telemtry, check length field)
+    technically DSM bind data is only 10 bytes but multi sends 16
+    like with telemtery, check length field)
 
   Type 0x06 Flysky AFHDS2 telemetry data
    length: 29
    data[0] = RSSI value
    data[1-28] telemetry data
 
+
+ Type 0x08 Input synchronisation
+    Informs the TX about desired rate and current delay
+    length: 4
+    data[0-1]     Desired refresh rate in µs
+    data[2-3]     Time (µs) between last serial servo input received and servo input needed (lateness), TX should adjust its
+                  sending time to minimise this value.
+	data[4]		  Interval of this message in ms
+ 	data[5]		  Input delay target in 10µs
+
+   Note that there are protocols (AFHDS2A) that have a refresh rate that is smaller than the maximum achievable
+   refresh rate via the serial protocol, in this case, the TX should double the rate and also subract this
+   refresh rate from the input lag if the input lag is more than the desired refresh rate.
+
+   The remote should try to get to zero of  (inputdelay+target*10).
+
+Commands from TX to module use values > 127 for command type
+
+ Type 0x80 Module Configuration
+   This sent from the TX to Multi to configure inversion and multi telemetry type
+   length: 1
+   data[0] flags
+     0x01 Telemetry inversion (1 = inverted)
+     0x02 Use Multi telemetry protocol (if 0 use multi status)
+     0x04 Send extra telemetry (type 0x08) to allow input synchronisation
+
+
+ Type 0x81 Failsafe data
+	length: 23
+	data[0] Failsafe mode:
+		 0 - Failsafe not set
+		 1 - Failsafe hold, keep last received values
+		 2 - Failsafe custom, use the values from the channels
+		 3 - Failsafe nopulses, stop sending pulses from the receiver
+		 4 - Failsafe receiver, use receiver stored values
+
+		Many of these many modes don't work with all protocols, fallback to best
+		available method
+	data[1-22] Failsafe data, encoded like normal channel data, with the expection
+		that 0 means hold for that channel and 2047 means no pulses
+
 */
\ No newline at end of file
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index 3ff4464..416f297 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -21,20 +21,23 @@
  along with Multiprotocol.  If not, see <http://www.gnu.org/licenses/>.
 */
 #include <avr/pgmspace.h>
+
 //#define DEBUG_TX
+//#define SERIAL_DEBUG		// Only for STM32_BOARD on usart1
+
 #define USE_MY_CONFIG
-#ifdef ARDUINO_AVR_XMEGA32D4
-#include "MultiOrange.h"
+
+#ifdef __arm__// Let's automatically select the board if arm is selected
+	#define STM32_BOARD
 #endif
+#ifdef ARDUINO_AVR_XMEGA32D4
+	#include "MultiOrange.h"
+#endif
+
 #include "Multiprotocol.h"
 
 //Multiprotocol module configuration file
 #include "_Config.h"
-// Let's automatically select the board
-// if arm is selected
-#ifdef __arm__
-	#define STM32_BOARD
-#endif
 
 //Personal config file
 #if defined USE_MY_CONFIG
@@ -58,6 +61,9 @@
 	void ISR_COMPB();
 	extern "C"
 	{
+		#ifdef SERIAL_DEBUG
+			void __irq_usart1(void);
+		#endif
 		void __irq_usart2(void);
 		void __irq_usart3(void);
 	}
@@ -184,6 +190,11 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets
 		volatile uint8_t tx_head=0;
 		volatile uint8_t tx_tail=0;
 	#endif // BASH_SERIAL
+	#ifdef SERIAL_DEBUG
+		volatile uint8_t tx_debug_buff[TXBUFFER_SIZE];
+		volatile uint8_t tx_debug_head=0;
+		volatile uint8_t tx_debug_tail=0;
+    #endif // SERIAL_DEBUG
 	uint8_t v_lipo1;
 	uint8_t v_lipo2;
 	uint8_t RX_RSSI;
@@ -193,7 +204,7 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets
 	uint8_t telemetry_link=0; 
 	uint8_t telemetry_counter=0;
 	uint8_t telemetry_lost;
-#endif 
+#endif // TELEMETRY
 
 // Callback
 typedef uint16_t (*void_function_t) (void);//pointer to a function with no parameters which return an uint16_t integer
@@ -202,6 +213,13 @@ void_function_t remote_callback = 0;
 // Init
 void setup()
 {
+	// Setup diagnostic uart before anything else
+	#ifdef SERIAL_DEBUG
+		usart1_begin(115200,SERIAL_8N1);
+		tx_debug_resume();
+		debug("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL);
+	#endif
+
 	// General pinout
 	#ifdef ORANGE_TX
 		//XMEGA
@@ -227,14 +245,15 @@ void setup()
 		pinMode(CC25_CSN_pin,OUTPUT);
 		pinMode(NRF_CSN_pin,OUTPUT);
 		pinMode(CYRF_CSN_pin,OUTPUT);
+		pinMode(SPI_CSN_pin,OUTPUT);
 		pinMode(CYRF_RST_pin,OUTPUT);
 		pinMode(PE1_pin,OUTPUT);
 		pinMode(PE2_pin,OUTPUT);
+		pinMode(TX_INV_pin,OUTPUT);
+		pinMode(RX_INV_pin,OUTPUT);
 		#if defined TELEMETRY
-			pinMode(TX_INV_pin,OUTPUT);
-			pinMode(RX_INV_pin,OUTPUT);
 			#if defined INVERT_SERIAL
-				TX_INV_on;//activated inverter for both serial TX and RX signals
+				TX_INV_on;	//activate inverter for both serial TX and RX signals
 				RX_INV_on;
 			#else
 				TX_INV_off;
@@ -287,7 +306,7 @@ void setup()
 		// Timer1 config
 		TCCR1A = 0;
 		TCCR1B = (1 << CS11);	//prescaler8, set timer1 to increment every 0.5us(16Mhz) and start timer
-		
+
 		// Random
 		random_init();
 	#endif
@@ -341,6 +360,7 @@ void setup()
 			((MODE_DIAL3_ipr & _BV(MODE_DIAL3_pin)) ? 0 : 4) +
 			((MODE_DIAL4_ipr & _BV(MODE_DIAL4_pin)) ? 0 : 8);
 	#endif
+    debug("Mode switch reads as %d", mode_select);
 
 	// Update LED
 	LED_off;
@@ -360,6 +380,8 @@ void setup()
 
 	// Read or create protocol id
 	MProtocol_id_master=random_id(10,false);
+
+	debug("Module Id: %lx", MProtocol_id_master);
 	
 #ifdef ENABLE_PPM
 	//Protocol and interrupts initialization
@@ -415,6 +437,7 @@ void setup()
 		#endif //ENABLE_SERIAL
 	}
 	servo_mid=servo_min_100+servo_max_100;	//In fact 2* mid_value
+	debug("init complete");
 }
 
 // Main
@@ -678,6 +701,18 @@ inline void tx_resume()
 	#endif
 }
 
+#ifdef SERIAL_DEBUG
+	inline void tx_debug_resume()
+	{
+		USART1_BASE->CR1 |= USART_CR1_TXEIE;
+	}
+
+	inline void tx_debug_pause()
+	{
+		USART1_BASE->CR1 &= ~ USART_CR1_TXEIE;
+	}
+#endif // SERIAL_DEBUG
+
 #ifdef STM32_BOARD	
 void start_timer2()
 {	
@@ -764,7 +799,7 @@ static void protocol_init()
 				#if defined(HUBSAN_A7105_INO)
 					case MODE_HUBSAN:
 						PE1_off;	//antenna RF1
-						if(IS_BIND_BUTTON_FLAG_on) random_id(10,true); // Generate new ID if bind button is pressed.
+						if(IS_BIND_BUTTON_FLAG_on) random_id(EEPROM_ID_OFFSET,true); // Generate new ID if bind button is pressed.
 						next_callback = initHubsan();
 						remote_callback = ReadHubsan;
 						break;
@@ -820,12 +855,12 @@ static void protocol_init()
 							{
 								if(IS_BIND_BUTTON_FLAG_on)
 								{
-									eeprom_write_byte((EE_ADDR)(30+mode_select),0x00);	// reset to autobind mode for the current model
+									eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x00);	// reset to autobind mode for the current model
 									option=0;
 								}
 								else
 								{	
-									option=eeprom_read_byte((EE_ADDR)(30+mode_select));	// load previous mode: autobind or fixed id
+									option=eeprom_read_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select));	// load previous mode: autobind or fixed id
 									if(option!=1) option=0;								// if not fixed id mode then it should be autobind
 								}
 							}
@@ -842,12 +877,12 @@ static void protocol_init()
 							{
 								if(IS_BIND_BUTTON_FLAG_on)
 								{
-									eeprom_write_byte((EE_ADDR)(30+mode_select),0x00);	// reset to autobind mode for the current model
+									eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x00);	// reset to autobind mode for the current model
 									option=0;
 								}
 								else
 								{	
-									option=eeprom_read_byte((EE_ADDR)(30+mode_select));	// load previous mode: autobind or fixed id
+									option=eeprom_read_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select));	// load previous mode: autobind or fixed id
 									if(option!=1) option=0;								// if not fixed id mode then it should be autobind
 								}
 							}
@@ -1051,6 +1086,7 @@ void update_serial_data()
 		protocol=(rx_ok_buff[0]==0x55?0:32) + (rx_ok_buff[1]&0x1F);	//protocol no (0-63) bits 4-6 of buff[1] and bit 0 of buf[0]
 		sub_protocol=(rx_ok_buff[2]>>4)& 0x07;	//subprotocol no (0-7) bits 4-6
 		RX_num=rx_ok_buff[2]& 0x0F;				// rx_num bits 0---3
+		debug("New protocol selected: %d, sub proto %d, rxnum %d", protocol, sub_protocol, RX_num);
 	}
 	else
 		if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) )		// Bind flag has been set
@@ -1070,7 +1106,7 @@ void update_serial_data()
 	//store current protocol values
 	for(uint8_t i=0;i<3;i++)
 		cur_protocol[i] =  rx_ok_buff[i];
-	
+
 	// decode channel values
 	volatile uint8_t *p=rx_ok_buff+3;
 	uint8_t dec=-3;
@@ -1363,7 +1399,7 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
 			{
 				id<<=8;
 				id|=eeprom_read_byte((EE_ADDR)address+i-1);
-			}	
+			}
 			if(id!=0x2AD141A7)	//ID with seed=0
 				return id;
 		}
@@ -1379,7 +1415,7 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
 		{
 			eeprom_write_byte((EE_ADDR)address+i,id);
 			id>>=8;
-		}	
+		}
 		eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom.
 		return id;
 	#else
diff --git a/Multiprotocol/Pins.h b/Multiprotocol/Pins.h
index 97765e5..e760180 100644
--- a/Multiprotocol/Pins.h
+++ b/Multiprotocol/Pins.h
@@ -207,14 +207,14 @@
 	#endif
 #else //STM32_BOARD
 	#define	BIND_pin		PA0
-	#define	LED_pin			PA1						
+	#define	LED_pin			PA1
 	//
-	#define	PPM_pin			PA8								//PPM  5V tolerant													
+	#define	PPM_pin			PA8								//PPM  5V tolerant
 	//
-	#define	S1_pin			PA4								//Dial switch pins	
-	#define	S2_pin			PA5							
+	#define	S1_pin			PA4								//Dial switch pins
+	#define	S2_pin			PA5
 	#define	S3_pin			PA6
-	#define	S4_pin			PA7	
+	#define	S4_pin			PA7
 	//
 	#define	PE1_pin			PB4								//PE1
 	#define	PE2_pin			PB5								//PE2
@@ -224,10 +224,11 @@
 	#define	CYRF_RST_pin	PB8								//CYRF RESET
 	#define	A7105_CSN_pin	PB9								//A7105
 	#define	CYRF_CSN_pin	PB12							//CYRF CSN
+	#define SPI_CSN_pin		PA15
 	//SPI pins	
 	#define	SCK_pin			PB13							//SCK
 	#define	SDO_pin			PB14							//MISO
-	#define	SDI_pin			PB15							//MOSI			
+	#define	SDI_pin			PB15							//MOSI
 	//
 	#define	TX_INV_pin		PB3
 	#define	RX_INV_pin		PB1
@@ -238,40 +239,43 @@
 	#define	PE2_on  		digitalWrite(PE2_pin,HIGH)
 	#define	PE2_off 		digitalWrite(PE2_pin,LOW)
 
-	#define	A7105_CSN_on	digitalWrite(A7105_CSN_pin,HIGH)			
-	#define	A7105_CSN_off	digitalWrite(A7105_CSN_pin,LOW)		
+	#define	A7105_CSN_on	digitalWrite(A7105_CSN_pin,HIGH)
+	#define	A7105_CSN_off	digitalWrite(A7105_CSN_pin,LOW)
 
 	#define NRF_CE_on
 	#define	NRF_CE_off
 
-	#define	SCK_on			digitalWrite(SCK_pin,HIGH)			
-	#define	SCK_off			digitalWrite(SCK_pin,LOW)		
+	#define	SCK_on			digitalWrite(SCK_pin,HIGH)
+	#define	SCK_off			digitalWrite(SCK_pin,LOW)
 
-	#define	SDI_on			digitalWrite(SDI_pin,HIGH)			
-	#define	SDI_off			digitalWrite(SDI_pin,LOW)		
+	#define	SDI_on			digitalWrite(SDI_pin,HIGH)
+	#define	SDI_off			digitalWrite(SDI_pin,LOW)
 
-	#define	SDI_1			(digitalRead(SDI_pin)==HIGH)	
-	#define	SDI_0			(digitalRead(SDI_pin)==LOW)			
+	#define	SDI_1			(digitalRead(SDI_pin)==HIGH)
+	#define	SDI_0			(digitalRead(SDI_pin)==LOW)
 
-	#define	CC25_CSN_on		digitalWrite(CC25_CSN_pin,HIGH)		
-	#define	CC25_CSN_off	digitalWrite(CC25_CSN_pin,LOW)	
+	#define	CC25_CSN_on		digitalWrite(CC25_CSN_pin,HIGH)
+	#define	CC25_CSN_off	digitalWrite(CC25_CSN_pin,LOW)
 
-	#define	NRF_CSN_on		digitalWrite(NRF_CSN_pin,HIGH)		
-	#define	NRF_CSN_off		digitalWrite(NRF_CSN_pin,LOW)	
+	#define	NRF_CSN_on		digitalWrite(NRF_CSN_pin,HIGH)
+	#define	NRF_CSN_off		digitalWrite(NRF_CSN_pin,LOW)
 
-	#define	CYRF_CSN_on		digitalWrite(CYRF_CSN_pin,HIGH)		
+	#define	CYRF_CSN_on		digitalWrite(CYRF_CSN_pin,HIGH)
 	#define	CYRF_CSN_off	digitalWrite(CYRF_CSN_pin,LOW)
 
-	#define	CYRF_RST_HI		digitalWrite(CYRF_RST_pin,HIGH)	//reset cyrf 
+	#define	SPI_CSN_on		digitalWrite(SPI_CSN_pin,HIGH)
+	#define	SPI_CSN_off		digitalWrite(SPI_CSN_pin,LOW)
+
+	#define	CYRF_RST_HI		digitalWrite(CYRF_RST_pin,HIGH)	//reset cyrf
 	#define	CYRF_RST_LO		digitalWrite(CYRF_RST_pin,LOW)	//
 
-	#define	SDO_1			(digitalRead(SDO_pin)==HIGH)		
-	#define	SDO_0			(digitalRead(SDO_pin)==LOW)	
+	#define	SDO_1			(digitalRead(SDO_pin)==HIGH)
+	#define	SDO_0			(digitalRead(SDO_pin)==LOW)
 
-	#define	TX_INV_on		digitalWrite(TX_INV_pin,HIGH)		
+	#define	TX_INV_on		digitalWrite(TX_INV_pin,HIGH)
 	#define	TX_INV_off		digitalWrite(TX_INV_pin,LOW)
 
-	#define	RX_INV_on		digitalWrite(RX_INV_pin,HIGH)		
+	#define	RX_INV_on		digitalWrite(RX_INV_pin,HIGH)
 	#define	RX_INV_off		digitalWrite(RX_INV_pin,LOW)
 
 	#define	LED_on			digitalWrite(LED_pin,HIGH)
diff --git a/Multiprotocol/Symax_nrf24l01.ino b/Multiprotocol/Symax_nrf24l01.ino
index b7fbb58..4df3f56 100644
--- a/Multiprotocol/Symax_nrf24l01.ino
+++ b/Multiprotocol/Symax_nrf24l01.ino
@@ -81,7 +81,7 @@ static void __attribute__((unused)) SYMAX_read_controls()
 	if (Servo_AUX5)
 	{
 		flags |= SYMAX_FLAG_HEADLESS;
-	    flags &= ~SYMAX_XTRM_RATES;	// Extended rates & headless incompatible
+		flags &= ~SYMAX_XTRM_RATES;	// Extended rates & headless incompatible
 	}
 }
 
@@ -158,7 +158,7 @@ static void __attribute__((unused)) SYMAX_build_packet(uint8_t bind)
 		packet[6] = flags & SYMAX_FLAG_FLIP ? 0x40 : 0x00;
 		packet[7] = flags & SYMAX_FLAG_HEADLESS ? 0x80 : 0x00;
 		if (flags & SYMAX_XTRM_RATES)
-		{	    // use trims to extend controls
+		{	// use trims to extend controls
 			packet[5] |= elevator >> 2;
 			packet[6] |= rudder >> 2;
 			packet[7] |= aileron >> 2;
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 8997161..52af18a 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -20,54 +20,47 @@
 uint8_t RetrySequence ;
 
 #if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
-	#define MULTI_TIME 500 //in ms
+	#define MULTI_TIME				500	//in ms
+	#define INPUT_SYNC_TIME			100	//in ms
+	#define INPUT_ADDITIONAL_DELAY	100	// in 10µs, 100 => 1000 µs
 	uint32_t lastMulti = 0;
-#endif
+#endif // MULTI_TELEMETRY/MULTI_STATUS
 
 #if defined SPORT_TELEMETRY	
-    #define SPORT_TIME 12000  //12ms
+	#define SPORT_TIME 12000	//12ms
 	#define FRSKY_SPORT_PACKET_SIZE   8
+	#define FX_BUFFERS	4
 	uint32_t last = 0;
 	uint8_t sport_counter=0;
 	uint8_t RxBt = 0;
 	uint8_t sport = 0;
-#define MAX_PKTX 10
-#define FX_BUFFERS	4
-uint8_t pktx[MAX_PKTX];
-uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS];
-uint8_t indx;
-//struct t_fx_rx_packet
-//{
-//	uint8_t validSequence ;
-//	uint8_t count ;
-//	uint8_t payload[6] ;
-//} ;
+	uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS];
 
-// Store for out of sequence packet
-//struct t_fx_rx_packet FrskyxRxTelemetry ;
+	// Store for out of sequence packet
+	uint8_t FrskyxRxTelemetryValidSequence ;
+	struct t_fx_rx_frame
+	{
+		uint8_t valid ;
+		uint8_t count ;
+		uint8_t payload[6] ;
+	} ;
 
-uint8_t FrskyxRxTelemetryValidSequence ;
+	// Store for FrskyX telemetry
+	struct t_fx_rx_frame FrskyxRxFrames[4] ;
+	uint8_t NextFxFrameToForward ;
+#endif // SPORT_TELEMETRY
 
-struct t_fx_rx_frame
-{
-	uint8_t valid ;
-	uint8_t count ;
-	uint8_t payload[6] ;
-} ;
-
-// Store for FrskyX telemetry
-struct t_fx_rx_frame FrskyxRxFrames[4] ;
-uint8_t NextFxFrameToForward ;
-
-#endif
 #if defined HUB_TELEMETRY
 	#define USER_MAX_BYTES 6
 	uint8_t prev_index;
-#endif
+#endif // HUB_TELEMETRY
 
-#define START_STOP              0x7e
-#define BYTESTUFF               0x7d
-#define STUFF_MASK              0x20
+#define START_STOP	0x7e
+#define BYTESTUFF	0x7d
+#define STUFF_MASK	0x20
+#define MAX_PKTX	10
+uint8_t pktx[MAX_PKTX];
+uint8_t indx;
 uint8_t frame[18];
 
 #if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
@@ -85,30 +78,30 @@ static void multi_send_header(uint8_t type, uint8_t len)
 
 static void multi_send_status()
 {
-    multi_send_header(MULTI_TELEMETRY_STATUS, 5);
+	multi_send_header(MULTI_TELEMETRY_STATUS, 5);
 
-    // Build flags
-    uint8_t flags=0;
-    if (IS_INPUT_SIGNAL_on)
-        flags |= 0x01;
-    if (mode_select==MODE_SERIAL)
-        flags |= 0x02;
-    if (remote_callback != 0)
-    {
-	    flags |= 0x04;
+	// Build flags
+	uint8_t flags=0;
+	if (IS_INPUT_SIGNAL_on)
+		flags |= 0x01;
+	if (mode_select==MODE_SERIAL)
+		flags |= 0x02;
+	if (remote_callback != 0)
+	{
+		flags |= 0x04;
 		if (IS_WAIT_BIND_on)
 			flags |= 0x10;
 		else
 			if (!IS_BIND_DONE_on)
 				flags |= 0x08;
 	}
-    Serial_write(flags);
+	Serial_write(flags);
 
-    // Version number example: 1.1.6.1
-    Serial_write(VERSION_MAJOR);
-    Serial_write(VERSION_MINOR);
-    Serial_write(VERSION_REVISION);
-    Serial_write(VERSION_PATCH_LEVEL);
+	// Version number example: 1.1.6.1
+	Serial_write(VERSION_MAJOR);
+	Serial_write(VERSION_MINOR);
+	Serial_write(VERSION_REVISION);
+	Serial_write(VERSION_PATCH_LEVEL);
 }
 #endif
 
@@ -156,9 +149,9 @@ static void multi_send_status()
 #ifdef MULTI_TELEMETRY
 static void multi_send_frskyhub()
 {
-    multi_send_header(MULTI_TELEMETRY_HUB, 9);
-    for (uint8_t i = 0; i < 9; i++)
-        Serial_write(frame[i]);
+	multi_send_header(MULTI_TELEMETRY_HUB, 9);
+	for (uint8_t i = 0; i < 9; i++)
+		Serial_write(frame[i]);
 }
 #endif
 
@@ -169,8 +162,8 @@ void frskySendStuffed()
 	{
 		if ((frame[i] == START_STOP) || (frame[i] == BYTESTUFF))
 		{
-			Serial_write(BYTESTUFF);	    	  
-			frame[i] ^= STUFF_MASK;	
+			Serial_write(BYTESTUFF);
+			frame[i] ^= STUFF_MASK;
 		}
 		Serial_write(frame[i]);
 	}
@@ -181,12 +174,8 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 {
 	uint8_t clen = pkt[0] + 3 ;
 	if(pkt[1] == rx_tx_addr[3] && pkt[2] == rx_tx_addr[2] && len == clen )
-	{	   
+	{
 		telemetry_link|=1;								// Telemetry data is available
-		/*previous version
-		RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>4);
-		if(pktt[len-2] >=128) RSSI_dBm -= 164;
-		else RSSI_dBm += 130;*/
 		TX_RSSI = pkt[len-2];
 		if(TX_RSSI >=128)
 			TX_RSSI -= 128;
@@ -204,12 +193,8 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 				{
 					uint8_t topBit = 0 ;
 					if ( telemetry_counter & 0x80 )
-					{
 						if ( ( telemetry_counter & 0x1F ) != RetrySequence )
-						{
 							topBit = 0x80 ;
-						}
-					}
 					telemetry_counter = ( (telemetry_counter+1)%32 ) | topBit ;	// Request next telemetry frame
 				}
 				else
@@ -222,28 +207,14 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 			}
 		}
 		else
-		{
 			pktt[6]=0; 									// Discard packet
-		}
 		//
 #if defined SPORT_TELEMETRY && defined FRSKYX_CC2500_INO
 		telemetry_lost=0;
 		if (protocol==MODE_FRSKYX)
 		{
 			uint16_t lcrc = crc_x(&pkt[3], len-7 ) ;
-//			if ( ( sub_protocol & 2 ) == 0 )
-//			{
-//				if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) )
-//				{
-//					lcrc = 0 ;
-//				}
-//				else
-//				{
-//					lcrc = 1 ;
-//				}
-//			}
-//			if ( lcrc == 0 )
-			
+
 			if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) )
 			{
 				// Check if in sequence
@@ -267,18 +238,12 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 					{
 						p->count = count ;
 						for ( uint8_t i = 0 ; i < count ; i += 1 )
-						{
 							p->payload[i] = pkt[i+7] ;
-						}
 					}
 					else
-					{
 						p->count = 0 ;
-					}
 					p->valid = 1 ;
-					
-					
-					
+		
 					FrX_receive_seq = ( FrX_receive_seq + 1 ) & 0x03 ;
 
 					if ( FrskyxRxTelemetryValidSequence & 0x80 )
@@ -287,24 +252,16 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 						FrskyxRxTelemetryValidSequence &= 0x7F ;
 					}
 
-//					if ( FrskyxRxTelemetry.validSequence & 0x80 )
-//					{
-//						FrX_receive_seq = ( FrskyxRxTelemetry.validSequence + 1 ) & 3 ;
-
-//						FrskyxRxTelemetry.validSequence &= 0x7F ;
-
-//					}
 				}
 				else
 				{
 					// Save and request correct packet
-//					struct t_fx_rx_packet *p ;
 					struct t_fx_rx_frame *q ;
 					uint8_t count ;
-		// pkt[4] RSSI
-		// pkt[5] sequence control
-		// pkt[6] payload count
-		// pkt[7-12] payload			
+					// pkt[4] RSSI
+					// pkt[5] sequence control
+					// pkt[6] payload count
+					// pkt[7-12] payload			
 					pktt[6] = 0 ; // Don't process
 					if ( (pkt[5] & 0x03) == ( ( FrX_receive_seq +1 ) & 3 ) )
 					{
@@ -319,55 +276,18 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 							}
 						}
 						else
-						{
 							q->count = 0 ;
-						}
 						q->valid = 1 ;
 					
 						FrskyxRxTelemetryValidSequence = 0x80 | ( pkt[5] & 0x03 ) ;
 					}
 					 
-					 
-//					p = &FrskyxRxTelemetry ;
-//					count = pkt[6] ;
-//					if ( count <= 6 )
-//					{
-//						p->count = count ;
-//						for ( uint8_t i = 0 ; i < count ; i += 1 )
-//						{
-//							p->payload[i] = pkt[i+7] ;
-//						}
-//						p->validSequence = 0x80 | ( pkt[5] & 0x03 ) ;
-//					}
-					FrX_receive_seq = ( FrX_receive_seq & 0x03 ) | 0x04 ;	// Request re-transmission
+					 FrX_receive_seq = ( FrX_receive_seq & 0x03 ) | 0x04 ;	// Request re-transmission
 				}
 
 				if (((pktt[5] >> 4) & 0x0f) == 0x08)
-				{
 					FrX_send_seq = 0 ;
-//					FrX_receive_seq = 0x08 ;
-				}
 			}
-			
-//			packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start
-//			if ( FrX_send_seq != 0x08 )
-//			{
-//				FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ;
-//			}
-			 
-//			if ((pktt[5] >> 4 & 0x0f) == 0x08)
-//			{  
-//				seq_last_sent = 8;
-//				seq_last_rcvd = 0;
-//				pass=0;
-//			} 
-//			else
-//			{
-//				if ((pktt[5] >> 4 & 0x03) == (seq_last_rcvd + 1) % 4)
-//					seq_last_rcvd = (seq_last_rcvd + 1) % 4;
-//				else
-//					pass=0;//reset if sequence wrong
-//			}
 		}
 #endif
 	}
@@ -446,7 +366,7 @@ void frsky_user_frame()
 	}
 	else
 		telemetry_link=0;
-}	   
+}
 /*
 HuB RX packets.
 pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09 
@@ -514,23 +434,19 @@ pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
 				
 		*/
 
-const uint8_t PROGMEM Indices[] = {	0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
-																		0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB,
-                                    0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71,
-                                    0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7,
-																		0x98, 0x39, 0xBA, 0x1B } ;
-
-
 #ifdef MULTI_TELEMETRY
+	const uint8_t PROGMEM Indices[] = {	0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
+										0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB,
+										0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71,
+										0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7,
+										0x98, 0x39, 0xBA, 0x1B } ;
 	void sportSend(uint8_t *p)
 	{
 		multi_send_header(MULTI_TELEMETRY_SPORT, 9);
 		uint16_t crc_s = 0;
 		uint8_t x = p[0] ;
 		if ( x <= 0x1B )
-		{
 			x = pgm_read_byte_near( &Indices[x] ) ;
-		}
 		Serial_write(x) ;
 		for (uint8_t i = 1; i < 9; i++)
 		{
@@ -540,8 +456,8 @@ const uint8_t PROGMEM Indices[] = {	0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
 
 			if (i>0)
 			{
-				crc_s += p[i]; //0-1FF
-				crc_s += crc_s >> 8; //0-100
+				crc_s += p[i];			//0-1FF
+				crc_s += crc_s >> 8;	//0-100
 				crc_s &= 0x00ff;
 			}
 		}
@@ -661,7 +577,7 @@ void proces_sport_data(uint8_t data)
 				pass = 1;
 				break;		
 			}
-			if(data == BYTESTUFF)//if they are stuffed
+			if(data == BYTESTUFF)	//if they are stuffed
 				pass=2;
 			else
 				if (indx < MAX_PKTX)		
@@ -680,9 +596,7 @@ void proces_sport_data(uint8_t data)
 			uint8_t dest = sport * FRSKY_SPORT_PACKET_SIZE ;
 			uint8_t i ;
 			for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i += 1 )
-			{
 				pktx1[dest++] = pktx[i] ;	// Triple buffer
-			}
 			sport += 1 ;//ok to send
 		}
 //		else
@@ -704,10 +618,9 @@ void TelemetryUpdate()
 		h = SerialControl.head ;
 		t = SerialControl.tail ;
 		if ( h >= t )
-			t += 192 - h ;
+			t += TXBUFFER_SIZE - h ;
 		else
 			t -= h ;
-//		if ( t < 32 )
 		if ( t < 64 )
 		{
 			return ;
@@ -741,9 +654,6 @@ void TelemetryUpdate()
 	#if defined SPORT_TELEMETRY
 		if (protocol==MODE_FRSKYX)
 		{	// FrSkyX
-//			struct t_fx_rx_frame *p ;
-//			uint8_t count ;
-			
 			for(;;)
 			{
 				struct t_fx_rx_frame *p ;
@@ -763,40 +673,12 @@ void TelemetryUpdate()
 				}
 			}
 			 
-//			p = &FrskyxRxFrames[NextFxFrameToForward] ;
-//			if ( p->valid )
-//			{
-//				count = p->count ;
-//				for (uint8_t i=0; i < count ; i++)
-//					proces_sport_data(p->payload[i]) ;
-//				p->valid = 0 ;	// Sent on
-//				NextFxFrameToForward = ( NextFxFrameToForward + 1 ) & 3 ;
-//			}
-			
 			if(telemetry_link)
 			{		
 				if(pktt[4] & 0x80)
 					RX_RSSI=pktt[4] & 0x7F ;
 				else 
 					RxBt = (pktt[4]<<1) + 1 ;
-				
-//				if(pktt[6] && pktt[6]<=6)
-//				{
-//					for (uint8_t i=0; i < pktt[6]; i++)
-//						proces_sport_data(pktt[7+i]);
-//					if ( FrskyxRxTelemetry.validSequence & 0x80 )
-//					{
-//						// Process out of sequence packet
-//						for (uint8_t i=0; i < FrskyxRxTelemetry.count ; i++)
-//						{
-//							proces_sport_data( FrskyxRxTelemetry.payload[i] ) ;
-//						}
-////						FrX_receive_seq = ( FrskyxRxTelemetry.validSequence + 1 ) & 3 ;
-//						FrskyxRxTelemetry.validSequence = 0 ;
-//					}
-//				}
-
-
 				telemetry_link=0;
 			}
 			uint32_t now = micros();
@@ -810,7 +692,7 @@ void TelemetryUpdate()
 				#endif
 			}
 		}
-	#endif					
+	#endif // SPORT_TELEMETRY
 
 	#if defined DSM_TELEMETRY
 		if(telemetry_link && protocol == MODE_DSM)
@@ -820,14 +702,14 @@ void TelemetryUpdate()
 			return;
 		}
 	#endif
-    #if defined AFHDS2A_FW_TELEMETRY     
-        if(telemetry_link == 2 && protocol == MODE_AFHDS2A)
+	#if defined AFHDS2A_FW_TELEMETRY
+		if(telemetry_link == 2 && protocol == MODE_AFHDS2A)
 		{
 			AFHDSA_short_frame();
 			telemetry_link=0;
 			return;
 		}
-    #endif        
+	#endif
 
 		if((telemetry_link & 1 )&& protocol != MODE_FRSKYX)
 		{	// FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell
@@ -850,6 +732,19 @@ void TelemetryUpdate()
 /**************************/
 /**************************/
 
+#ifdef SERIAL_DEBUG
+	void StatusSerial_write(uint8_t data)
+	{
+		uint8_t nextHead ;
+		nextHead = tx_debug_head + 1 ;
+		if ( nextHead >= TXBUFFER_SIZE )
+			nextHead = 0 ;
+		tx_debug_buff[nextHead]=data;
+		tx_debug_head = nextHead ;
+		tx_debug_resume();
+	}
+#endif // SERIAL_DEBUG
+
 #ifndef BASH_SERIAL
 	// Routines for normal serial output
 	void Serial_write(uint8_t data)
@@ -969,11 +864,34 @@ void TelemetryUpdate()
 		#endif		
 	}
 	#ifdef STM32_BOARD
+		#if defined(SERIAL_DEBUG)
+			void __irq_usart1()
+			{	// Transmit interrupt
+				if(USART1_BASE->SR & USART_SR_TXE)
+				{
+					if(tx_debug_head!=tx_debug_tail)
+					{
+						if(++tx_debug_tail>=TXBUFFER_SIZE)				//head
+							tx_debug_tail=0;
+						USART1_BASE->DR=tx_debug_buff[tx_debug_tail];	//clears TXE bit
+					}
+					if (tx_debug_tail == tx_debug_head)
+						tx_debug_pause(); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
+				}
+			}
+			void usart1_begin(uint32_t baud,uint32_t config )
+			{
+				usart_init(USART1);
+				usart_config_gpios_async(USART1,GPIOA,PIN_MAP[PA10].gpio_bit,GPIOA,PIN_MAP[PA9].gpio_bit,config);
+				usart_set_baud_rate(USART1, STM32_PCLK1, baud);
+				usart_enable(USART1);
+			}
+		#endif
 		void usart2_begin(uint32_t baud,uint32_t config )
 		{
 			usart_init(USART2); 
 			usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config);
-			usart_set_baud_rate(USART2, STM32_PCLK1, baud);//
+			usart_set_baud_rate(USART2, STM32_PCLK1, baud);
 			usart_enable(USART2);
 		}
 		void usart3_begin(uint32_t baud,uint32_t config )
@@ -1056,7 +974,9 @@ void Serial_write( uint8_t byte )
 	#ifdef INVERT_SERIAL
 		byte |= 1 ;		// Start bit
 	#endif
-	uint8_t next = (SerialControl.head + 2) & 0x7f ;
+	uint8_t next = SerialControl.head + 2;
+	if(next>TXBUFFER_SIZE)
+		next=0;
 	if ( next != SerialControl.tail )
 	{
 		SerialControl.data[SerialControl.head] = byte ;
@@ -1120,9 +1040,7 @@ ISR(TIMER0_COMPA_vect)
 		GPIOR1 = 3 ;
 	}
 	else
-	{
 		OCR0A += 20 ;
-	}
 }
 
 ISR(TIMER0_COMPB_vect)
@@ -1150,12 +1068,9 @@ ISR(TIMER0_COMPB_vect)
 			{
 				GPIOR0 = ptr->data[ptr->tail] ;
 				GPIOR2 = ptr->data[ptr->tail+1] ;
-				uint8_t nextTail ;
-				nextTail = ptr->tail + 2 ;
-				if ( nextTail > 192 )
-				{
+				uint8_t nextTail = ptr->tail + 2 ;
+				if ( nextTail > TXBUFFER_SIZE )
 					nextTail = 0 ;
-				}
 				ptr->tail = nextTail ;
 				GPIOR1 = 8 ;
 				OCR0A = OCR0B + 40 ;
@@ -1170,44 +1085,36 @@ ISR(TIMER0_COMPB_vect)
 		}
 	}
 	else
-	{
 		OCR0B += 20 ;
-	}
 }
 
 ISR(TIMER0_OVF_vect)
 {
 	uint8_t byte ;
 	if ( GPIOR1 > 2 )
-	{
 		byte = GPIOR0 ;
-	}
 	else
-	{
 		byte = GPIOR2 ;
-	}
 	if ( byte & 0x01 )
 		SERIAL_TX_on;
 	else
 		SERIAL_TX_off;
 	byte /= 2 ;		// Generates shorter code than byte >>= 1
 	if ( GPIOR1 > 2 )
-	{
 		GPIOR0 = byte ;
-	}
 	else
-	{
 		GPIOR2 = byte ;
-	}
 	if ( --GPIOR1 == 0 )
-	{
-		// prepare next byte
+	{	// prepare next byte
 		volatile struct t_serial_bash *ptr = &SerialControl ;
 		if ( ptr->head != ptr->tail )
 		{
 			GPIOR0 = ptr->data[ptr->tail] ;
 			GPIOR2 = ptr->data[ptr->tail+1] ;
-			ptr->tail = ( ptr->tail + 2 ) & 0x7F ;
+			uint8_t nextTail = ptr->tail + 2 ;
+			if ( nextTail > TXBUFFER_SIZE )
+				nextTail = 0 ;
+			ptr->tail = nextTail ;
 			GPIOR1 = 10 ;
 		}
 		else
diff --git a/Multiprotocol/WK2x01_cyrf6936.ino b/Multiprotocol/WK2x01_cyrf6936.ino
index a5cd83b..59dc71b 100644
--- a/Multiprotocol/WK2x01_cyrf6936.ino
+++ b/Multiprotocol/WK2x01_cyrf6936.ino
@@ -294,7 +294,7 @@ static void __attribute__((unused)) WK_build_beacon_pkt_2801()
 		BIND_SET_PULLUP;										// set pullup
 		if(IS_BIND_BUTTON_on)
 		{
-			eeprom_write_byte((EE_ADDR)(30+mode_select),0x01);	// Set fixed id mode for the current model
+			eeprom_write_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+mode_select),0x01);	// Set fixed id mode for the current model
 			option=1;
 		}
 		BIND_SET_OUTPUT;
diff --git a/docs/Compiling.md b/docs/Compiling.md
index 29a5085..1f85ca9 100644
--- a/docs/Compiling.md
+++ b/docs/Compiling.md
@@ -1,50 +1,61 @@
 # Compiling and Programming (ATmega 328P)
 
-Multiprotocol source are compiled using the well known Arduino IDE.
+Multiprotocol firmware is compiled using the Arduino IDE.  The guide below will walk you through all the steps to compile and upload your customized firmware.
 
-The procedure below will guide you through all the steps to upload successfully a customized firmware.
+## Index
+1. [Tools Required](#tools-required)
+1. [Preparation](#preparation)
+   1. [Install the Arduino IDE](#install-the-arduino-ide)
+   1. [Download the Multiprotocol source and open the project](#download-the-multiprotocol-source-and-open-the-project)
+   1. [Install the Multi 4-in-1 board](#install-the-multi-4-in-1-board)
+   1. [Configure the Arduino IDE](#configure-the-arduino-ide)
+1. [Configure the firmware](#configure-the-firmware)
+   1. [Customize the firmware to match your hardware and your needs](#customize-the-firmware-to-match-your-hardware-and-your-needs)
+   1. [Verify the firmware](#verify-the-firmware)
+1. [Compiling and uploading the firmware](#compiling-and-uploading-the-firmware)
+   1. [Connect the programmer](#connect-the-programmer)
+   1. [Burn bootloader and set fuses](#burn-bootloader-and-set-fuses)
+   1. [Upload the firmware](#upload-the-firmware)
+      1. [Flash from TX](#flash-from-tx)
+      1. [Upload using Arduino IDE](#upload-using-arduino-ide)
+1. [Troubleshooting](#troubleshooting)
 
-## Install the Arduino IDE and the Multiprotocol project firmware
-1. Download and install the Arduino IDE. The currently supported Arduino version is 1.6.12. available for [Windows]( https://www.arduino.cc/download_handler.php?f=/arduino-1.6.12-windows.exe) and [Mac OSX](https://www.arduino.cc/download_handler.php?f=/arduino-1.6.12-macosx.zip)
+## Tools required
+
+| **3.3V USBasp Programmer** | **10-pin to 6-pin Adapter** | **6-pin header** |
+|:---:|:---:|:---:|
+| <img src="images/USBasp_Programmer.jpeg" width="200" height="200"/> | <img src="images/10pin_2_6pin.JPG" width="150" height="150"/> | <img src="images/6pin_header.jpg" width="100" height="100"/> |
+| [(example aliexpress link)](https://www.aliexpress.com/item/USBasp-USB-ISP-3-3V-5V-AVR-Programmer-USB-ATMEGA8-ATMEGA128-New-10PIN-Wire-Support/2036402518.html?spm=2114.30010308.8.10.jIbHzs) | [(example ebay link)](http://www.ebay.fr/itm/10-Pin-a-6-Pin-Carte-Adaptateur-M-F-pour-AVRISP-USBASP-STK500-Noir-Bleu-WT-/291862396761?hash=item43f45abf59:g:gXsAAOSwMgdXyGnh) | [(example Digi-Key link)](http://www.digikey.com/products/en?keywords=3M%20961206-6404-AR) |
+
+**Important:** The USBasp **must** be **3.3V**.  Using a 5V USBasp will fry the RF modules as they are not 5V tolerant.
+
+**Tip**: You can cut or remove the VCC line on your USBasp 6-pin adapter or ribbon cable and power the module from the radio when flashing to ensure that it receives the correct voltage.
+
+The 6-pin header needs to be soldered onto the board as indicated by the red rectangle:
+
+| **Banggood 4-in-1 Module** | **DIY Multiprotocol Module** | **Arduino Pro Mini Module** |
+|:---:|:---:|:---:|
+| <img src="images/V2b_ISP.jpeg" width="189" height="200"/> | <img src="images/MPTM_PCB_2.3d_ISP.png" width="486" height="201"/> | <img src="images/ProMini_ISP.png" width="195" height="200"/> |
+
+## Preparation
+### Install the Arduino IDE
+1. Download and install the Arduino IDE. The currently supported Arduino version is 1.8.5, available for [Windows]( https://www.arduino.cc/download_handler.php?f=/arduino-1.8.5-windows.exe) and [Mac OSX](https://www.arduino.cc/download_handler.php?f=/arduino-1.8.5-macosx.zip)
 1. It is recommended to upgrade Java to the [latest version](https://www.java.com/en/download/)
-1. Download the zip file with the Multiprotocol module source code from [here](https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/archive/master.zip)
-1. Unzip and copy the source code folder **Multiprotocol** to a folder of your choosing
-1. Click on the **Multiprotocol.ino** file in the **Multiprotocol** folder and the Arduino environment should appear and the Multiprotocol project will be loaded.
 
-## Upload the firmware
+### Download the Multiprotocol source and open the project
+1. Either
+   1. Download the zip file with the Multiprotocol module source code from [here](https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/archive/master.zip) and unzip and copy the source code folder **Multiprotocol** to a location of your choosing, or
+   1. Clone the project using Git or Github Desktop, then
+1. Double-click the **Multiprotocol.ino** file in the **Multiprotocol** folder to open the project in the Arduino IDE
 
-### Material you need to upload the firmware
+### Install the Multi 4-in-1 board
+1. Follow [these instructions](/BootLoaders/README.md) to install the **Multi 4-in-1 AVR Board** in the Arduino IDE
 
-1. USBASP programmer supporting 3.3V: <br> <img src="images/USBasp_Programmer.jpeg" width="200" height="200"/> <br> [(example aliexpress link)](https://www.aliexpress.com/item/USBasp-USB-ISP-3-3V-5V-AVR-Programmer-USB-ATMEGA8-ATMEGA128-New-10PIN-Wire-Support/2036402518.html?spm=2114.30010308.8.10.jIbHzs) <br> There are reports that some of the cheap programmers are not safe to use with 3.3V units, usually the black PCB versions are ok. <br>
-1. 10pin to 6pin adapter: <br> <img src="images/10pin_2_6pin.JPG" width="150" height="150"/> <br> [(example ebay link)](http://www.ebay.fr/itm/10-Pin-a-6-Pin-Carte-Adaptateur-M-F-pour-AVRISP-USBASP-STK500-Noir-Bleu-WT-/291862396761?hash=item43f45abf59:g:gXsAAOSwMgdXyGnh) <br>
-1. 6 pin header like this one: <br> <img src="images/6pin_header.jpg" width="100" height="100"/> <br> [(example Digi-Key link)](http://www.digikey.com/products/en?keywords=3M%20961206-6404-AR) <br>
-1. The 6 Pin header needs to be solder on the board like indicated by the red rectangle:
-   * Banggood readymade 4-in-1 module: <br><img src="images/V2b_ISP.jpeg" width="189" height="200"/> <br>
-   * DIY Mulitprotocol modules (like the 2.3d board): <br><img src="images/MPTM_PCB_2.3d_ISP.png" width="486" height="201"/> <br>
-   * Arduino Pro Mini module: <br><img src="images/ProMini_ISP.png" width="195" height="200"/> <br>
+### Configure the Arduino IDE
+1. Under **Tools -> Board** select **'Multi 4-in-1 (Atmega328p, 3.3V, 16MHz)**
+1. Under **Tools -> Programmer** select **USBasp**
 
-### Connect the programmer
-
-1. Before you connect the programmer make sure that you have selected the 3.3V mode and not 5V. The RF Modules are not 5V tolerant and you will break them with 5V.  On most programmers this is done by moving a jumper. <br> <img src="images/USBasp_Programmer_jumper.png" width="200" height="200" />
-1. Please re-read item 1. above before going on.
-1. Turn the rotary switch on the DIY Multiprotocol module to the 0 position. If you do not have a switch for Serial mode only then it is the same as being in the 0 position. The upload will not work if the switch is in any other position.
-1. Connect the 6-pin programming connector to the 6-pin ASP IVR connector on the DIY Multiprotocol board. Be sure to match the ground pin of the programmer connector to the ground pin on the board.
-
-The images below indicates the pin layout and the location of the ground pin on the board:
-* Banggood readymade 4-in-1 module: <br> <img src="images/V2b_ISP.jpeg" width="189" height="200" /> <br>
-* DIY Mulitprotocol modules (like the 2.3d board): <br> <img src="images/MPTM_PCB_2.3d_ISP.png" width="486" height="201" /> <br>
-* Arduino Pro Mini module: <br> <img src="images/ProMini_ISP.png" width="195" height="200" /> <br>
-
-You are now ready to plug in the USB programmer to the computer
-
-If you are looking for a good working USBASP Windows driver, [use this one](http://www.protostack.com/download/USBasp-win-driver-x86-x64-v3.0.7.zip).
-
-### Configure Arduino IDE for Multiprotocol
-1. Under Tools -> Board select the Arduino Pro or Pro Mini
-1. Under Tools -> Processor select the ATmega328 (5V, 16MHz)
-1. Under Tools -> Programmer select your programmer type (probably USBASP from the shopping list above)
-
-<a name="CustomizeFirmareToYourNeeds"></a>
+## Configure the firmware
 ### Customize the firmware to match your hardware and your needs
 All customization is done by editing the ```_Config.h  ``` file in the Multiprotocol Arduino project.  
 
@@ -56,95 +67,82 @@ Most of the default settings should get you started quickly. But on modules with
 
 To fill in the "PROTOCOLS TO INCLUDE" section, it would be good to review all the available protocols on the [Protocol Details](../Protocols_Details.md) page and identify which one you would like to add on your module.  
 
-To check that the program will compile correctly and fit in the Atmega press the Check mark as shown below. <br> <img src="images/Arduino_check.jpg" width="99" height="130" />
+### Verify the firmware
+
+To check that the program will compile correctly and fit in the Atmega click **Sketch -> Verify/Compile**, or press **Ctrl+R**.
 
 If you see something like the following, your firmware is still too big and you need to deselect additional protocols:
-> Sketch uses 34,096 bytes (104%) of program storage space. Maximum is 32,768 bytes.
-> Global variables use 1,236 bytes (60%) of dynamic memory, leaving 812 bytes for local variables. Maximum is 2,048 bytes.
-> Sketch too big.
+```
+Sketch uses 42032 bytes (128%) of program storage space. Maximum is 32768 bytes.
+
+Global variables use 1180 bytes (57%) of dynamic memory, leaving 868 bytes for local variables. Maximum is 2048 bytes.
+Sketch too big; see http://www.arduino.cc/en/Guide/Troubleshooting#size for tips on reducing it.
+Error compiling for board Multi 4-in-1 (Atmega328p, 3.3V, 16MHz).
+```
 
 If there is another error carefully read it, go to the line number indicated and correct your typo.
 
-### Flash the firmware
+If there are no errors and you see output like this:
+```
+Sketch uses 31874 bytes (97%) of program storage space. Maximum is 32768 bytes.
+Global variables use 1083 bytes (52%) of dynamic memory, leaving 965 bytes for local variables. Maximum is 2048 bytes.
+```
+You can proceed to the next step.
 
-1. If you have a 4in1 Multiprotocol module you can skip this step. If you've just finished to build your DIY Multiprotocol module (like v2.3d), the first step is to flash the fuses of the microcontroller. This needs to be done only once. For this purpose, click on **Tools -> Burn Bootloader** 
-1. You are now ready to flash the firmware.  In the Arduino IDE click **Sketch -> Upload Using Programmer**.
+## Compiling and uploading the firmware
+If you have already burned the bootloader, and are simply recompiling firmware to re-flash using your TX, you can skip straight to [Flash from TX](#flash-from-tx).
 
-If the output indicates that the firmware has been uploaded successfully - give yourself a pat on the back.  Well done, you have successfully programmed your DIY Multiprotocol module. You can already go to the final step [Setting up your Transmitter](Transmitters.md#compatible-transmitters) and begin to fly!!!! But don't forget to visit the next topic [Advanced settings](#AdvancedSettings) which has some extra steps needed to use your module at his full potential.
+### Connect the programmer
+1. Before you connect the programmer make sure that you have selected the 3.3V mode and not 5V. The RF Modules are not 5V tolerant and you will break them with 5V.  On most programmers this is done by moving a jumper. <br> <img src="images/USBasp_Programmer_jumper.png" width="200" height="200" />
+1. Please re-read item 1. above before going on, it's important.
+1. If your module has a rotary switch, set it to the 0 position. The upload will not work if the switch is in any other position.
+1. Connect the 6-pin programming connector to the 6-pin ASP IVR connector on the DIY Multiprotocol board. Be sure to match the ground pin of the programmer connector to the ground pin on the board.
 
-**Troubleshooting**
+The images below indicate the pin layout and the location of the ground pin on the board:
 
-If you get an error that indicates "warning : Can not Set sck period . usbasp please check for firmware update ." just ignore it, everything is fine.
+| **Banggood 4-in-1 Module** | **DIY Multiprotocol Module** | **Arduino Pro Mini Module** |
+|:---:|:---:|:---:|
+<img src="images/V2b_ISP.jpeg" width="189" height="200" /> | <img src="images/MPTM_PCB_2.3d_ISP.png" width="486" height="201" /> | <img src="images/ProMini_ISP.png" width="195" height="200" /> |
+
+You are now ready to plug in the USB programmer to the computer.  If you are looking for a good working USBasp Windows driver, [use this one](http://www.protostack.com/download/USBasp-win-driver-x86-x64-v3.0.7.zip).
+
+### Burn bootloader and set fuses
+The bootloader only needs to be burned once, unless you decide to switch from one option to the other (or it is accidentally erased).  If you have already burned the bootloader / set the fuses you can skip this step.
+
+There are two bootloader options:
+   * **'No bootloader'** maximises flash space for protocols
+   * **'Flash from TX'** (highly recommended) installs a small (512 byte) bootloader which allows flashing the module firmware using from a radio running ersky9x
+
+**Note:** 'Burning the bootloader' is necessary even if the 'No bootloader' option is selected, as it sets the fuses on the AVR module.  This only needs to be once (unless you decide to change your bootloader choice later)
+
+1. Under **Tools -> Bootloader** select a bootloader
+1. Click on **Tools -> Burn Bootloader** 
+
+### Upload the firmware
+You are now ready to upload the firmware to the multiprotocol module.  There are two methods for uploading the firmware:
+* **Flash from TX** uses the maintenance mode in radios running ersky9x to upload the firmware
+* **Upload using Arduino IDE** uses the Arduino IDE and the USBasp programmer to upload the firmware
+
+**Note:** 'Flash from TX' is only available with radios running ersky9x r221e2 or newer.
+
+#### Flash from TX
+1. In the Arduino IDE click **Sketch -> Export compiled Binary**, or press **Ctrl+Alt+S**
+1. Locate the file named **multifw.hex** in the **Multiprotocol** folder
+1. Follow the instructions [here](/docs/Flash_from_Tx.md) to upload the firmware using your radio
+
+You can disconnect the programmer now as it is not needed any more.
+
+#### Upload using Arduino IDE
+**Note:** If you have burned the 'Upload from TX' bootloader and you then upload firmware to your module using **Upload Using Programmer**, you will erase the bootloader.  That's just the way the Arduino IDE works - avrdude will erase the entire flash memory prior to writing the new code, *including the bootloader*, and the upload will not put it back.  If this happens you can [burn it again](#burn-bootloader-and-set-fuses).
+
+1. In the Arduino IDE click **Sketch -> Upload Using Programmer**, or press **Ctrl+Shift+U**.
+
+If the output indicates that the firmware has been uploaded successfully - give yourself a pat on the back.  Well done, you have successfully programmed your DIY Multiprotocol module. You can already go to the final step [Setting up your Transmitter](Transmitters.md#compatible-transmitters) and begin to fly!!!!
+
+## Troubleshooting
+If you get an error that indicates "warning : Can not Set sck period . usbasp please check for firmware update ." just ignore it, everything is fine.  Don't be tempted to 'upgrade' your USBasp firmware to try to get rid of this message - if you do, you will find that the USBasp is no longer able to flash your multiprotocol module.
 
 If you get an error that indicates a valid microcontroller was not found there is something wrong with:
- - your connections, 
- - your programmer, or
- - your board
-
-<a name="AdvancedSettings"></a>
-## Advanced settings
-
-So you followed the previous steps and your module is working.
-Below are some extra steps which will:
- - Prevent the EEPROM from being erased each time the firmware is flashed. This will preserve your Tx ID and save you from having to rebind all your models after an update of the firmware.
- - Permit to flash even more protocols (extra 2KB)
-
-### Arduino Boards.txt modification
-First, we need to append some text to the Arduino file boards.txt.
-
-#### On Windows
-1. Close the Arduino IDE
-1. Search Windows for the application WordPad (DO NOT USE Notepad). <br> Right click on WordPad and select "Run as Administrator": <br> <img src="images/WordPad_Admin.jpg" height="200" /> <br>
-1. Open the file ```boards.txt``` located in this folder ```C:\Program Files(x86)\Arduino\hardware\arduino\avr ``` or the equivalent if you have installed Aduino in a different directory.
-1. Append the following text into the end of the file and save it:
-   ```
-   ##############################################################
-   ## Multi 4-in-1 (3.3V, 16 MHz) w/ ATmega328
-   ## --------------------------------------------------
-   multi.name=Multi 4-in-1
-   
-   multi.upload.tool=avrdude
-   multi.upload.protocol=arduino
-   
-   multi.bootloader.tool=avrdude
-   multi.bootloader.unlock_bits=0x3F
-   multi.bootloader.lock_bits=0x0F
-   
-   multi.build.board=AVR_PRO
-   multi.build.core=arduino
-   multi.build.variant=eightanaloginputs
-   multi.build.extra_flags=-Wl,--relax
-   
-   multi.menu.cpu.16MHzatmega328=ATmega328 (3.3V, 16 MHz)
-   
-   multi.menu.cpu.16MHzatmega328.upload.maximum_size=32768
-   multi.menu.cpu.16MHzatmega328.upload.maximum_data_size=2048
-   multi.menu.cpu.16MHzatmega328.upload.speed=57600
-   
-   multi.menu.cpu.16MHzatmega328.bootloader.low_fuses=0xFF
-   multi.menu.cpu.16MHzatmega328.bootloader.high_fuses=0xD3
-   multi.menu.cpu.16MHzatmega328.bootloader.extended_fuses=0xFD
-   
-   multi.menu.cpu.16MHzatmega328.build.mcu=atmega328p
-   multi.menu.cpu.16MHzatmega328.build.f_cpu=16000000L
-   ##############################################################   
-   ```
-
-#### On Mac OSX:
-1. Close the Arduino IDE
-1. Using finder navigate to ```Applications``` folder
-1. Ctl-Click on the Arduino application and select **Show Package Contents**.
-1. Browse to ```Contents/Java/hardware/arduino/avr``` and double click on boards.txt
-1. Copy and paste the "Multi 4-in-1" text listed above into the end of the file and save it.
-
-### Burn Bootloader
-1. Open the Arduino IDE and load the Multiprotocol project.
-1. Select under **Tools -> Board** the new entry **Multi 4-in-1**
-1. Select under **Tools -> Programmer** the entry **USBasp**
-1. Click on **Tools -> Burn Bootloader**.  Do not worry it will return an error that no bootloader was found. In fact we are interrested by the first few lines indicating that the fuses were set correctly.
-1. At this stage your flash module is empty so it's normal if the status LED does not do anything.
-
-### Flash the firmware
-Scroll back to the section [Customize the firmware to your hardware and your needs](#CustomizeFirmareToYourNeeds) above and follow the instructions.
-
-You are done good fly!!!
+ * your connections, 
+ * your programmer, or
+ * your board
diff --git a/docs/Flash_from_Tx.md b/docs/Flash_from_Tx.md
new file mode 100644
index 0000000..8cf2e45
--- /dev/null
+++ b/docs/Flash_from_Tx.md
@@ -0,0 +1,33 @@
+# Flashing from the Transmitter
+
+For radios running ersky9x r221e2 or newer, there is an option to flash a precompiled firmware file to the multiprotocol module using the transmitter's Maintenance Mode.
+
+## Tools required
+* A compatible transmitter running ersky9x r221e2, or newer
+* A precompiled multiprotocol firmware file (.hex for Atmega328p or .bin for STM32)
+* A **Flash from TX** bootloader installed on the multiprotocol module
+* A means to get the firmware file onto the transmitter's SD card
+
+Consult the [ersky9x site](http://www.er9x.com/) to see if your transmitter is compatible.
+
+The transmitter firmware can be downloaded from the [ersky9x test firmware page](http://openrcforums.com/forum/viewtopic.php?f=7&t=4676).
+
+## Procedure
+1. Either:
+   1. Connect the transmitter using a USB cable and power it on, or 
+   1. Remove the SD card from the transmitter and mount it using a suitable reader
+1. Copy the pre-compiled firmware file into the **\firmware** folder of the SD card (create the folder if it does not exist)
+1. Power the transmitter off and remove the USB cable or put the SD card back in the transmitter
+1. Enter the transmitter's Maintenance Menu by powering it on with the outer buttons of the two horizontal trims held down
+1. Select **Update Multi**,
+1. Choose the appropriate file type
+   1. **HEX** to update an Atmega328p module
+   1. **BIN** to update an STM32 module
+1. Select **Update**
+1. Choose the firmware file to flash, long press to select it
+1. Long press again to flash the selected file to the module
+
+When flashing has finished, long press EXIT to reboot in normal mode.
+
+## Troubleshooting
+TBD