Failsafe: default for PPM and set on radio for Serial

2025-12-14 03:23:15 +00:00 · 2017-12-07 16:02:18 +01:00
parent ebd44d9628
commit 4418cab3a5
12 changed files with 255 additions and 237 deletions
--- a/Multiprotocol/AFHDS2A_a7105.ino
+++ b/Multiprotocol/AFHDS2A_a7105.ino
@@ -179,30 +179,19 @@ static void AFHDS2A_build_packet(uint8_t type)
 			packet[0] = 0x56;
 			for(uint8_t ch=0; ch<14; ch++)
 			{
-#ifdef AFHDS2A_FAILSAFE
-				int8_t failsafe = AFHDS2AFailsafe[ch];
-				//
-				if(failsafe != -1)
-				{
-					//
-					if (failsafe > AFHDS2AFailsafeMAX)
-						failsafe = AFHDS2AFailsafeMAX;
-					//
-					if (failsafe < AFHDS2AFailsafeMIN)
-						failsafe = AFHDS2AFailsafeMIN;
-					//
-					double scale = (float)failsafe/(float)100;
-					int16_t failsafeMicros = 1500 + ((float)512 * scale);
-					//
-					packet[9 + ch*2] =  failsafeMicros & 0xff;
-					packet[10+ ch*2] = ( failsafeMicros >> 8) & 0xff;
-				}
-				else
-#endif
-				{
-					packet[9 + ch*2] = 0xff;
-					packet[10+ ch*2] = 0xff;
-				}
+				#ifdef FAILSAFE_ENABLE
+					uint16_t failsafeMicros = (Failsafe_data[CH_AETR[ch]]*5)/8+860;
+					if( failsafeMicros!=FAILSAFE_CHANNEL_HOLD+860)
+					{ // Failsafe values
+						packet[9 + ch*2] =  failsafeMicros & 0xff;
+						packet[10+ ch*2] = ( failsafeMicros >> 8) & 0xff;
+					}
+					else
+				#endif
+					{ // no values
+						packet[9 + ch*2] = 0xff;
+						packet[10+ ch*2] = 0xff;
+					}
 			}
 			break;
 		case AFHDS2A_PACKET_SETTINGS:
@@ -307,10 +296,15 @@ uint16_t ReadAFHDS2A()
 				hopping_frequency_no = 0;
 			if(!(packet_counter % 1313))
 				packet_type = AFHDS2A_PACKET_SETTINGS;
-			else if(!(packet_counter % 1569))
-				packet_type = AFHDS2A_PACKET_FAILSAFE;
 			else
-				packet_type = AFHDS2A_PACKET_STICKS;		// todo : check for settings changes
+			{
+				#ifdef FAILSAFE_ENABLE
+					if(!(packet_counter % 1569) && IS_FAILSAFE_VALUES_on)
+						packet_type = AFHDS2A_PACKET_FAILSAFE;
+					else
+				#endif
+						packet_type = AFHDS2A_PACKET_STICKS;		// todo : check for settings changes
+			}
 			if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)) && data_rx==1)
 			{ // RX+FECF+CRCF Ok
 				A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
--- a/Multiprotocol/Common.ino
+++ b/Multiprotocol/Common.ino
@@ -12,6 +12,21 @@
 You should have received a copy of the GNU General Public License
 along with Multiprotocol.  If not, see <http://www.gnu.org/licenses/>.
 */
+
+#ifdef FAILSAFE_ENABLE
+void InitFailsafe()
+{
+	for(uint8_t i=0;i<NUM_CHN;i++)
+		Failsafe_data[i]=1024;
+	Failsafe_data[THROTTLE]=FAILSAFE_THROTTLE_LOW;	//0=-125%, 204=-100%
+	FAILSAFE_VALUES_on;
+	#ifdef FAILSAFE_SERIAL_ONLY
+		if(mode_select == MODE_SERIAL)
+			FAILSAFE_VALUES_off;
+	#endif
+}
+#endif
+
 /************************/
 /**  Convert routines  **/
 /************************/
@@ -54,6 +69,15 @@ void convert_channel_HK310(uint8_t num, uint8_t *low, uint8_t *high)
 	*low=(uint8_t)(temp&0xFF);
 	*high=(uint8_t)(temp>>8);
 }
+// Failsafe value is converted for HK310
+#ifdef FAILSAFE_ENABLE
+void convert_failsafe_HK310(uint8_t num, uint8_t *low, uint8_t *high)
+{
+	uint16_t temp=0xFFFF-(3440+((Failsafe_data[num]*5)>>1))/3;
+	*low=(uint8_t)(temp&0xFF);
+	*high=(uint8_t)(temp>>8);
+}
+#endif

 // Channel value is converted to 16bit values
 uint16_t convert_channel_16b(uint8_t num, int16_t out_min, int16_t out_max)
--- a/Multiprotocol/Devo_cyrf6936.ino
+++ b/Multiprotocol/Devo_cyrf6936.ino
@@ -94,15 +94,29 @@ static void __attribute__((unused)) DEVO_add_pkt_suffix()
 static void __attribute__((unused)) DEVO_build_beacon_pkt(uint8_t upper)
 {
 	packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x07;
-	uint8_t max = 8;
+	uint8_t max = 8, offset = 0, enable = 0;
 	if (upper)
 	{
 		packet[0] += 1;
 		max = 4;
+		offset = 8;
 	}
 	for(uint8_t i = 0; i < max; i++)
-		packet[i+1] = 0;
-	packet[9] = 0;
+	{
+		#ifdef FAILSAFE_ENABLE
+			uint16_t failsafe=Failsafe_data[CH_EATR[i+offset]];
+			if(i + offset < DEVO_NUM_CHANNELS && failsafe!=FAILSAFE_CHANNEL_HOLD && IS_FAILSAFE_VALUES_on)
+			{
+				enable |= 0x80 >> i;
+				packet[i+1] = ((failsafe*25)>>8)-100;
+			}
+			else
+		#else
+			(void)offset;
+		#endif
+				packet[i+1] = 0;
+	}
+	packet[9] = enable;
 	DEVO_add_pkt_suffix();
 }

--- a/Multiprotocol/FrSkyX_cc2500.ino
+++ b/Multiprotocol/FrSkyX_cc2500.ino
@@ -19,14 +19,13 @@

 #include "iface_cc2500.h"

-uint8_t chanskip;
-//uint8_t seq_last_sent;
-//uint8_t seq_last_rcvd;
-
+uint8_t FrX_chanskip;
 uint8_t FrX_send_seq ;
 uint8_t FrX_receive_seq ;

-static void __attribute__((unused)) set_start(uint8_t ch )
+#define FRX_FAILSAFE_TIMEOUT 1032
+
+static void __attribute__((unused)) frskyX_set_start(uint8_t ch )
 {
 	CC2500_Strobe(CC2500_SIDLE);
 	CC2500_WriteReg(CC2500_25_FSCAL1, calData[ch]);
@@ -48,7 +47,7 @@ static void __attribute__((unused)) frskyX_init()
 	//#######END INIT########		
 }

-static void __attribute__((unused)) initialize_data(uint8_t adr)
+static void __attribute__((unused)) frskyX_initialize_data(uint8_t adr)
 {
 	CC2500_WriteReg(CC2500_0C_FSCTRL0,option);	// Frequency offset hack 
 	CC2500_WriteReg(CC2500_18_MCSM0,    0x8);	
@@ -57,28 +56,28 @@ static void __attribute__((unused)) initialize_data(uint8_t adr)
 }

 //**CRC**
-const uint16_t PROGMEM CRC_Short[]={
+const uint16_t PROGMEM frskyX_CRC_Short[]={
 	0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
 	0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7 };
-static uint16_t CRCTable(uint8_t val)
+static uint16_t __attribute__((unused)) frskyX_CRCTable(uint8_t val)
 {
 	uint16_t word ;
-	word = pgm_read_word(&CRC_Short[val&0x0F]) ;
+	word = pgm_read_word(&frskyX_CRC_Short[val&0x0F]) ;
 	val /= 16 ;
 	return word ^ (0x1081 * val) ;
 }
-static uint16_t __attribute__((unused)) crc_x(uint8_t *data, uint8_t len)
+static uint16_t __attribute__((unused)) frskyX_crc_x(uint8_t *data, uint8_t len)
 {
 	uint16_t crc = 0;
 	for(uint8_t i=0; i < len; i++)
-		crc = (crc<<8) ^ CRCTable((uint8_t)(crc>>8) ^ *data++);
+		crc = (crc<<8) ^ frskyX_CRCTable((uint8_t)(crc>>8) ^ *data++);
 	return crc;
 }

 // 0-2047, 0 = 817, 1024 = 1500, 2047 = 2182
 //64=860,1024=1500,1984=2140//Taranis 125%

-static uint16_t  __attribute__((unused)) scaleForPXX( uint8_t i )
+static uint16_t  __attribute__((unused)) frskyX_scaleForPXX( uint8_t i )
 {	//mapped 860,2140(125%) range to 64,1984(PXX values);
 	return (uint16_t)(((Servo_data[i]-servo_min_125)*3)>>1)+64;
 }
@@ -103,7 +102,7 @@ static void __attribute__((unused)) frskyX_build_bind_packet()
 	//
 	uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
 	memset(&packet[13], 0, limit - 13);
-	uint16_t lcrc = crc_x(&packet[3], limit-3);
+	uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3);
 	//
 	packet[limit++] = lcrc >> 8;
 	packet[limit] = lcrc;
@@ -124,8 +123,8 @@ static void __attribute__((unused)) frskyX_data_frame()
 	packet[2] = rx_tx_addr[2];
 	packet[3] = 0x02;
 	//  
-	packet[4] = (chanskip<<6)|hopping_frequency_no; 
-	packet[5] = chanskip>>2;
+	packet[4] = (FrX_chanskip<<6)|hopping_frequency_no; 
+	packet[5] = FrX_chanskip>>2;
 	packet[6] = RX_num;
 	//packet[7] = FLAGS 00 - standard packet
 	//10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
@@ -138,12 +137,12 @@ static void __attribute__((unused)) frskyX_data_frame()
 	
 	for(uint8_t i = 0; i <12 ; i+=3)
 	{//12 bytes
-		chan_0 = scaleForPXX(startChan);		 
+		chan_0 = frskyX_scaleForPXX(startChan);		 
 		if(lpass & 1 )
 			chan_0+=2048;			
 		startChan+=1;
 		//
-		chan_1 = scaleForPXX(startChan);		
+		chan_1 = frskyX_scaleForPXX(startChan);		
 		if(lpass & 1 )
 			chan_1+= 2048;		
 		startChan+=1;
@@ -187,8 +186,8 @@ static void __attribute__((unused)) frskyX_data_frame()
 			Serial.println(" ");
 		#endif
 	#endif // SPORT_POLLING
-	uint16_t lcrc = crc_x(&packet[3], limit-3);
-	
+
+	uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3);
 	packet[limit++]=lcrc>>8;//high byte
 	packet[limit]=lcrc;//low byte
 }
@@ -198,7 +197,7 @@ uint16_t ReadFrSkyX()
 	switch(state)
 	{	
 		default: 
-			set_start(47);		
+			frskyX_set_start(47);		
 			CC2500_SetPower();
 			CC2500_Strobe(CC2500_SFRX);
 			//		
@@ -211,7 +210,7 @@ uint16_t ReadFrSkyX()
 				state++;
 			return 9000;
 		case FRSKY_BIND_DONE:
-			initialize_data(0);
+			frskyX_initialize_data(0);
 			hopping_frequency_no=0;
 			BIND_DONE;
 			state++;			
@@ -223,10 +222,10 @@ uint16_t ReadFrSkyX()
 				prev_option = option ;
 			}
 			CC2500_SetTxRxMode(TX_EN);
-			set_start(hopping_frequency_no);
+			frskyX_set_start(hopping_frequency_no);
 			CC2500_SetPower();		
 			CC2500_Strobe(CC2500_SFRX);
-			hopping_frequency_no = (hopping_frequency_no+chanskip)%47;
+			hopping_frequency_no = (hopping_frequency_no+FrX_chanskip)%47;
 			CC2500_Strobe(CC2500_SIDLE);		
 			CC2500_WriteData(packet, packet[0]+1);
 			//
@@ -287,8 +286,8 @@ uint16_t initFrSkyX()
 	set_rx_tx_addr(MProtocol_id_master);
 	Frsky_init_hop();
 	packet_count=0;
-	while(!chanskip)
-		chanskip=random(0xfefefefe)%47;
+	while(!FrX_chanskip)
+		FrX_chanskip=random(0xfefefefe)%47;

 	//for test***************
 	//rx_tx_addr[3]=0xB3;
@@ -299,12 +298,12 @@ uint16_t initFrSkyX()
 	if(IS_AUTOBIND_FLAG_on)
 	{	   
 		state = FRSKY_BIND;
-		initialize_data(1);
+		frskyX_initialize_data(1);
 	}
 	else
 	{
 		state = FRSKY_DATA1;
-		initialize_data(0);
+		frskyX_initialize_data(0);
 	}
 //	seq_last_sent = 0;
 //	seq_last_rcvd = 8;
--- a/Multiprotocol/Hisky_nrf24l01.ino
+++ b/Multiprotocol/Hisky_nrf24l01.ino
@@ -150,12 +150,25 @@ uint16_t hisky_cb()
 			case 4:
 				phase=6;
 				break;
-			case 7:		// build packet with failsafe every 100ms
-				convert_channel_HK310(hopping_frequency_no!=0?RUDDER:AUX2,&packet[0],&packet[1]);
-				convert_channel_HK310(hopping_frequency_no!=0?THROTTLE:AUX3,&packet[2],&packet[3]);
-				convert_channel_HK310(hopping_frequency_no!=0?AUX1:AUX4,&packet[4],&packet[5]);
-				packet[7]=hopping_frequency_no!=0?0x55:0xAA;
-				packet[8]=hopping_frequency_no!=0?0x67:0x5A;
+			case 7:	// build packet
+				#ifdef FAILSAFE_ENABLE
+					if(IS_FAILSAFE_VALUES_on && hopping_frequency_no==0)
+					{ //  send failsafe every 100ms
+						convert_failsafe_HK310(RUDDER,  &packet[0],&packet[1]);
+						convert_failsafe_HK310(THROTTLE,&packet[2],&packet[3]);
+						convert_failsafe_HK310(AUX1,    &packet[4],&packet[5]);
+						packet[7]=0xAA;
+						packet[8]=0x5A;
+					}
+					else
+				#endif
+					{
+						convert_channel_HK310(RUDDER,  &packet[0],&packet[1]);
+						convert_channel_HK310(THROTTLE,&packet[2],&packet[3]);
+						convert_channel_HK310(AUX1,    &packet[4],&packet[5]);
+						packet[7]=0x55;
+						packet[8]=0x67;
+					}
 				phase=8;
 				break;
 		}
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -245,24 +245,8 @@ enum MultiPacketTypes {
 	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")

@@ -301,9 +285,9 @@ enum FailSafeMode {
 #define BIND_DONE			protocol_flags |= _BV(7)
 #define IS_BIND_DONE_on		( ( protocol_flags & _BV(7) ) !=0 )
 //
-#define BAD_PROTO_off		protocol_flags2 &= ~_BV(0)
-#define BAD_PROTO_on		protocol_flags2 |= _BV(0)
-#define IS_BAD_PROTO_on		( ( protocol_flags2 & _BV(0) ) !=0 )
+#define FAILSAFE_VALUES_off	protocol_flags2 &= ~_BV(0)
+#define FAILSAFE_VALUES_on		protocol_flags2 |= _BV(0)
+#define IS_FAILSAFE_VALUES_on	( ( protocol_flags2 & _BV(0) ) !=0 )
 //
 #define RX_DONOTUPDTAE_off	protocol_flags2 &= ~_BV(1)
 #define RX_DONOTUPDTAE_on	protocol_flags2 |= _BV(1)
@@ -336,21 +320,15 @@ enum FailSafeMode {
 #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)
+#define FAILSAFE_CHANNEL_HOLD		0
+#define	FAILSAFE_CHANNEL_NOPULSES	2047

-
-//Status messages
+//Debug messages
 #if defined(STM32_BOARD) && defined (SERIAL_DEBUG)
 	#define debug(msg, ...)  {char buf[64]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.write(buf);}
 #else
-	#define debug(...)
+	#define debug(...) { }
 	#undef SERIAL_DEBUG
 #endif

@@ -365,7 +343,6 @@ enum FailSafeMode {
 #define BLINK_WAIT_BIND_TIME_HIGH	1000
 #define BLINK_WAIT_BIND_TIME_LOW	100

-
 //*******************
 //***  AUX flags  ***
 //*******************
@@ -509,8 +486,10 @@ enum {
 **************************
 Serial: 100000 Baud 8e2      _ xxxx xxxx p --
  Total of 26 bytes
-  Stream[0]   = 0x55	sub_protocol values are 0..31
-  Stream[0]   = 0x54	sub_protocol values are 32..63
+  Stream[0]   = 0x55	sub_protocol values are 0..31	Stream contains channels
+  Stream[0]   = 0x54	sub_protocol values are 32..63	Stream contains channels
+  Stream[0]   = 0x57	sub_protocol values are 0..31	Stream contains failsafe
+  Stream[0]   = 0x56	sub_protocol values are 32..63	Stream contains failsafe
   header
  Stream[1]   = sub_protocol|BindBit|RangeCheckBit|AutoBindBit;
   sub_protocol is 0..31 (bits 0..4), value should be added with 32 if Stream[0] = 0x54
@@ -671,14 +650,16 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
   Power value => 0x80	0=High/1=Low
  Stream[3]   = option_protocol;
   option_protocol value is -128..127
-  Stream[4] to [25] = Channels
+  Stream[4] to [25] = Channels or failsafe depending on Steam[0]
   16 Channels on 11 bits (0..2047)
 	0		-125%
    204		-100%
 	1024	   0%
 	1843	+100%
 	2047	+125%
-   Channels bits are concatenated to fit in 22 bytes like in SBUS protocol
+   Values are concatenated to fit in 22 bytes like in SBUS protocol.
+   Failsafe values have exactly the same range/values than normal channels except the extremes where
+      0=hold, 2047=no pulse. If failsafe is not set or RX then failsafe packets should not be sent.
 */
 /*
  Multimodule Status
@@ -764,45 +745,4 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
   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
-
 */
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -79,6 +79,9 @@ uint16_t Servo_data[NUM_CHN];
 uint8_t  Servo_AUX;
 uint16_t servo_max_100,servo_min_100,servo_max_125,servo_min_125;
 uint16_t servo_mid;
+#ifdef FAILSAFE_ENABLE
+	uint16_t Failsafe_data[NUM_CHN];
+#endif

 // Protocol variables
 uint8_t  cyrfmfg_id[6];//for dsm2 and devo
@@ -101,6 +104,7 @@ uint8_t  flags;
 uint16_t crc;
 uint8_t  crc8;
 uint16_t seed;
+uint16_t failsafe_count;
 //
 uint16_t state;
 uint8_t  len;
@@ -329,14 +333,6 @@ void setup()
 		SCLK_off;
 	#endif

-	// Set servos positions
-	for(uint8_t i=0;i<NUM_CHN;i++)
-		Servo_data[i]=1500;
-	Servo_data[THROTTLE]=servo_min_100;
-	#ifdef ENABLE_PPM
-		memcpy((void *)PPM_data,Servo_data, sizeof(Servo_data));
-	#endif
-	
 	//Wait for every component to start
 	delayMilliseconds(100);
 	
@@ -359,6 +355,14 @@ void setup()
 	#endif
    debug("Mode switch reads as %d", mode_select);

+	// Set default channels' value
+	for(uint8_t i=0;i<NUM_CHN;i++)
+		Servo_data[i]=1500;
+	Servo_data[THROTTLE]=servo_min_100;
+	#ifdef ENABLE_PPM
+		memcpy((void *)PPM_data,Servo_data, sizeof(Servo_data));
+	#endif
+
 	// Update LED
 	LED_off;
 	LED_output; 
@@ -754,6 +758,10 @@ static void protocol_init()
 		MProtocol_id = RX_num + MProtocol_id_master;
 		set_rx_tx_addr(MProtocol_id);
 		
+		#ifdef FAILSAFE_ENABLE
+			InitFailsafe();
+		#endif
+	
 		blink=millis();

 		if(IS_BIND_BUTTON_FLAG_on)
@@ -1077,7 +1085,16 @@ void update_serial_data()
 		POWER_FLAG_on;							//power high

 	option=rx_ok_buff[3];
-
+	
+	#ifdef FAILSAFE_ENABLE
+		bool failsafe=false;
+		if(rx_ok_buff[0]&0x02)
+		{ //packet contains failsafe instead of channels
+			failsafe=true;
+			rx_ok_buff[0]&=0xFD;					//remove the failsafe flag
+			FAILSAFE_VALUES_on;						//failsafe data has been received
+		}
+	#endif
 	if( (rx_ok_buff[0] != cur_protocol[0]) || ((rx_ok_buff[1]&0x5F) != (cur_protocol[1]&0x5F)) || ( (rx_ok_buff[2]&0x7F) != (cur_protocol[2]&0x7F) ) )
 	{ // New model has been selected
 		CHANGE_PROTOCOL_FLAG_on;				//change protocol
@@ -1106,7 +1123,7 @@ void update_serial_data()
 	for(uint8_t i=0;i<3;i++)
 		cur_protocol[i] =  rx_ok_buff[i];

-	// decode channel values
+	// decode channel/failsafe values
 	volatile uint8_t *p=rx_ok_buff+3;
 	uint8_t dec=-3;
 	for(uint8_t i=0;i<NUM_CHN;i++)
@@ -1118,7 +1135,13 @@ void update_serial_data()
 			p++;
 		}
 		p++;
-		Servo_data[i]=((((*((uint32_t *)p))>>dec)&0x7FF)*5)/8+860;	//value range 860<->2140 -125%<->+125%
+		uint16_t temp=((*((uint32_t *)p))>>dec)&0x7FF;
+		#ifdef FAILSAFE_ENABLE
+			if(failsafe)
+				Failsafe_data[i]=temp;				//value range 0..2047, 0=hold, 2047=no pulses
+			else
+		#endif
+				Servo_data[i]=(temp*5)/8+860;		//value range 860<->2140 -125%<->+125%
 	}
 	RX_DONOTUPDTAE_off;
 	#ifdef ORANGE_TX
@@ -1136,6 +1159,10 @@ void update_serial_data()
 	#else
 		UCSR0B |= _BV(RXCIE0) ;					// RX interrupt enable
 	#endif
+	#ifdef FAILSAFE_ENABLE
+		if(failsafe)
+			debug("RX_FS:%d,%d,%d,%d",Failsafe_data[0],Failsafe_data[1],Failsafe_data[2],Failsafe_data[3]);
+	#endif
 }

 void modules_reset()
@@ -1501,7 +1528,11 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
 				idx=0;discard_frame=0;
 				RX_MISSED_BUFF_off;			// If rx_buff was good it's not anymore...
 				rx_buff[0]=UDR0;
-				if((rx_buff[0]&0xFE)==0x54)	// If 1st byte is 0x54 or 0x55 it looks ok
+				#ifdef FAILSAFE_ENABLE
+					if((rx_buff[0]&0xFC)==0x54)	// If 1st byte is 0x54, 0x55, 0x56 or 0x57 it looks ok
+				#else
+					if((rx_buff[0]&0xFE)==0x54)	// If 1st byte is 0x54 or 0x55 it looks ok
+				#endif
 				{
 					TX_RX_PAUSE_on;
 					tx_pause();
--- a/Multiprotocol/SFHSS_cc2500.ino
+++ b/Multiprotocol/SFHSS_cc2500.ino
@@ -127,7 +127,7 @@ static void __attribute__((unused)) SFHSS_calc_next_chan()
 // Values grow down and to the right.
 static void __attribute__((unused)) SFHSS_build_data_packet()
 {
-	uint16_t ch1,ch2,ch3,ch4;
+	uint16_t ch[4];
 	// command.bit0 is the packet number indicator: =0 -> SFHSS_DATA1, =1 -> SFHSS_DATA2
 	// command.bit1 is unknown but seems to be linked to the payload[0].bit0 but more dumps are needed: payload[0]=0x82 -> =0, payload[0]=0x81 -> =1
 	// command.bit2 is the failsafe transmission indicator: =0 -> normal data, =1->failsafe data
@@ -139,44 +139,53 @@ static void __attribute__((unused)) SFHSS_build_data_packet()
 	
 	uint8_t command= (phase == SFHSS_DATA1) ? 0 : 1;	// Building packet for Data1 or Data2
 	counter+=command;
-	if( (counter&0x3FC) == 0x3FC )
-	{	// Transmit failsafe data twice every 7s
-		if( ((counter&1)^(command&1)) == 0 )
-			command|=0x04;								// Failsafe
-	}
-	else
-		command|=0x02;									// Assuming packet[0] == 0x81
+	#ifdef FAILSAFE_ENABLE
+		if( (counter&0x3FC) == 0x3FC && IS_FAILSAFE_VALUES_on)
+		{	// Transmit failsafe data twice every 7s
+			if( ((counter&1)^(command&1)) == 0 )
+				command|=0x04;							// Failsafe
+		}
+		else
+	#endif
+			command|=0x02;								// Assuming packet[0] == 0x81
 	counter&=0x3FF;										// Reset failsafe counter
 	if(counter&1) command|=0x08;						// Transmit lower and upper channels twice in a row

-	uint8_t ch_offset = ((command&0x08) >> 1) | ((command&0x04) << 1);	// CH1..CH4 or CH5..CH8, if failsafe CH9..CH12 or CH13..CH16
-	ch1 = convert_channel_16b_nolim(CH_AETR[ch_offset+0],2020,1020);
-	ch2 = convert_channel_16b_nolim(CH_AETR[ch_offset+1],2020,1020);
-	ch3 = convert_channel_16b_nolim(CH_AETR[ch_offset+2],2020,1020);
-	ch4 = convert_channel_16b_nolim(CH_AETR[ch_offset+3],2020,1020);
+	uint8_t ch_offset = (command&0x08) >> 1;			// CH1..CH4 or CH5..CH8

-	if(command&0x04)
-	{	//Failsafe data are:
-		// 0 to 1023 -> no output on channel
-		// 1024-2047 -> hold output on channel
-		// 2048-4095 -> channel_output=(data&0x3FF)*5/4+880 in µs
-		// Notes:
-		//    2048-2559 -> does not look valid since it only covers the range from 1520µs to 2160µs 
-		//    2560-3583 -> valid for any channel values from 880µs to 2160µs
-		//    3584-4095 -> looks to be used for the throttle channel with values ranging from 880µs to 1520µs
-		#ifdef SFHSS_FAILSAFE_CH9_16
-			ch1=((5360-ch1)<<2)/5;						//((1520*2-ch1)<<2)/5+1856;
-			ch2=((5360-ch2)<<2)/5;
-			ch3=((5360-ch3)<<2)/5;
-			if((command&0x08)==0 && ch3<3072)			// Throttle
-				ch3+=1024;
-			ch4=((5360-ch4)<<2)/5;
-		#else
-			ch1=1024;ch2=1024;ch4=1024;					// All channels hold their positions
-			ch3=((command&0x08)==0)?3664:1024;			// except throttle value set to 980µs
-		#endif
-	}
+	#ifdef FAILSAFE_ENABLE
+		if(command&0x04)
+		{	//Failsafe data are:
+			// 0 to 1023 -> no output on channel
+			// 1024-2047 -> hold output on channel
+			// 2048-4095 -> channel_output=(data&0x3FF)*5/4+880 in µs
+			// Notes:
+			//    2048-2559 -> does not look valid since it only covers the range from 1520µs to 2160µs 
+			//    2560-3583 -> valid for any channel values from 880µs to 2160µs
+			//    3584-4095 -> looks to be used for the throttle channel with values ranging from 880µs to 1520µs
+			for(uint8_t i=0;i<4;i++)
+			{
+				ch[i]=Failsafe_data[CH_AETR[ch_offset+i]];
+				if(ch[i]==FAILSAFE_CHANNEL_HOLD)
+					ch[i]=1024;
+				else if(ch[i]==FAILSAFE_CHANNEL_NOPULSES)
+					ch[i]=0;
+				else
+				{ //Use channel value
+					ch[i]=(ch[i]>>1)+2560;
+					if(CH_AETR[ch_offset+i]==THROTTLE && ch[i]<3072)		// Throttle
+						ch[i]+=1024;
+				}
+			}
+		}
+		else
+	#endif
+		{	//Normal data
+			for(uint8_t i=0;i<4;i++)
+				ch[i] = convert_channel_16b_nolim(CH_AETR[ch_offset+i],2020,1020);
+		}

+	
 	// XK		[0]=0x81 [3]=0x00 [4]=0x00
 	// T8J		[0]=0x81 [3]=0x42 [4]=0x07
 	// T10J		[0]=0x81 [3]=0x0F [4]=0x09
@@ -186,13 +195,13 @@ static void __attribute__((unused)) SFHSS_build_data_packet()
 	packet[2] = rx_tx_addr[1];
 	packet[3] = 0x00;	// unknown but prevents some receivers to bind if not 0
 	packet[4] = 0x00;	// unknown but prevents some receivers to bind if not 0
-	packet[5] = (rf_ch_num << 3) | ((ch1 >> 9) & 0x07);
-	packet[6] = (ch1 >> 1);
-	packet[7] = (ch1 << 7) | ((ch2 >> 5) & 0x7F );
-	packet[8] = (ch2 << 3) | ((ch3 >> 9) & 0x07 );
-	packet[9] = (ch3 >> 1);
-	packet[10] = (ch3 << 7) | ((ch4 >> 5) & 0x7F );
-	packet[11] = (ch4 << 3) | ((fhss_code >> 2) & 0x07 );
+	packet[5] = (rf_ch_num << 3) | ((ch[0] >> 9) & 0x07);
+	packet[6] = (ch[0] >> 1);
+	packet[7] = (ch[0] << 7) | ((ch[1] >> 5) & 0x7F );
+	packet[8] = (ch[1] << 3) | ((ch[2] >> 9) & 0x07 );
+	packet[9] = (ch[2] >> 1);
+	packet[10] = (ch[2] << 7) | ((ch[3] >> 5) & 0x7F );
+	packet[11] = (ch[3] << 3) | ((fhss_code >> 2) & 0x07 );
 	packet[12] = (fhss_code << 6) | command;
 }

@@ -285,7 +294,6 @@ uint16_t initSFHSS()

 	SFHSS_rf_init();
 	phase = SFHSS_START;
-
 	return 10000;
 }

--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -228,7 +228,7 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 		telemetry_lost=0;
 		if (protocol==MODE_FRSKYX)
 		{
-			uint16_t lcrc = crc_x(&pkt[3], len-7 ) ;
+			uint16_t lcrc = frskyX_crc_x(&pkt[3], len-7 ) ;

 			if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) )
 			{
--- a/Multiprotocol/WK2x01_cyrf6936.ino
+++ b/Multiprotocol/WK2x01_cyrf6936.ino
@@ -317,8 +317,17 @@ static void __attribute__((unused)) WK_build_beacon_pkt_2801()
        bind_state = 0x99;
 	
 	for (uint8_t i = 0; i < 4; i++)
-	{	// failsafe info: WARNING All channels are set to 0 instead of midstick and 0 for throttle
-		packet[i+1] = 0;
+	{
+		#ifdef FAILSAFE_ENABLE
+			uint16_t failsafe=Failsafe_data[CH_AETR[i + WK_last_beacon * 4]];
+			if(failsafe!=FAILSAFE_CHANNEL_HOLD && IS_FAILSAFE_VALUES_on)
+			{
+				packet[i+1] = failsafe>>3;	//0..255
+				en |= 1 << i;
+			}
+			else
+		#endif
+				packet[i+1] = 0;
 	}
 	packet[0] = en;
 	packet[5] = packet[4];
@@ -453,7 +462,8 @@ uint16_t WK_cb()
 	return 1200;
 }

-uint16_t WK_setup() {
+uint16_t WK_setup()
+{
 	wk2x01_cyrf_init();
 	CYRF_SetTxRxMode(TX_EN);

--- a/Multiprotocol/_Config.h
+++ b/Multiprotocol/_Config.h
@@ -158,42 +158,23 @@
 /**************************/
 /*** FAILSAFE SETTINGS  ***/
 /**************************/
-//SHFSS failsafe is by default set to all channels hold their positions except throttle forced to low (980µs)
-//You can uncomment the setting below to use channels 9(1) to 16(8) as failsafe instead
-//#define SFHSS_FAILSAFE_CH9_16
+//The module is using the same default failsafe values for all protocols which currently supports it:
+//  Devo, SFHSS and AFHDS2A
+//All channels are centered except throttle which is forced low.
+//If you want to diasble failsafe globally comment the line below using "//".
+#define FAILSAFE_ENABLE

-#define AFHDS2A_FAILSAFE
-#ifdef AFHDS2A_FAILSAFE
-/*
-	Failsafe Min/Max values 962 <-> 2038
-*/
-const int8_t AFHDS2AFailsafeMIN = -105;
-const int8_t AFHDS2AFailsafeMAX = 105;
-//
-const int8_t AFHDS2AFailsafe[14]=	{
-/*
- Failsafe examples
- 988 <-> 2012µs -100% =  988 = 1500 + (2012-988)/2 * (-100/100) = 1500 - 512 =  988
- 988 <-> 2012µs    0% = 1500 = 1500 + (2012-988)/2 * (   0/100) = 1500 +   0 = 1500
- 988 <-> 2012µs  100% = 2012 = 1500 + (2012-988)/2 * ( 100/100) = 1500 + 512 = 2012
- 988 <-> 2012µs -105% =  962 = 1500 + (2012-988)/2 * (-105/100) = 1500 - 538 =  962
-*/
-/* ch  1 */ -1,
-/* ch  2 */ -1,
-/* ch  3 */ -105,
-/* ch  4 */ -1,
-/* ch  5 */ -1,
-/* ch  6 */ -1,
-/* ch  7 */ -1,
-/* ch  8 */ -1,
-/* ch  9 */ -1,
-/* ch 10 */ -1,
-/* ch 11 */ -1,
-/* ch 12 */ -1,
-/* ch 13 */ -1,
-/* ch 14 */ -1
-};
-#endif
+//Failsafe throttle low value.
+//1=-125%, 204=-100%, 1024=0%, 1843=100%, 2046=+125%
+#define FAILSAFE_THROTTLE_LOW 204
+
+//The radio using serial protocol can set failsafe data (ersky9x only for now).
+// Two options are available:
+//  a. replace the default failsafe data with serial failsafe data when they are received.
+//  b. wait for the radio to provide failsafe before sending it. Enable advanced settings like "FAILSAFE NOT SET" or "FAILSAFE RX".
+// Option a. is the default since you have a protection even if no failsafe has been set on the radio.
+// You can force option b. by uncommenting the line below (remove the "//").
+//#define FAILSAFE_SERIAL_ONLY

 /**************************/
 /*** TELEMETRY SETTINGS ***/