From 9c35f6f73ca48056509c4cbd44a49614d87d4dca Mon Sep 17 00:00:00 2001
From: Pascal Langer <planger@mathworks.com>
Date: Mon, 1 Feb 2021 11:54:16 +0100
Subject: [PATCH] Futaba S-FHSS: Fist attempt to fix failsafe

---
 Multiprotocol/Futaba_cc2500.ino | 10 ++++------
 Multiprotocol/Multiprotocol.h   |  2 +-
 2 files changed, 5 insertions(+), 7 deletions(-)

diff --git a/Multiprotocol/Futaba_cc2500.ino b/Multiprotocol/Futaba_cc2500.ino
index 4d15242..a4db1dd 100644
--- a/Multiprotocol/Futaba_cc2500.ino
+++ b/Multiprotocol/Futaba_cc2500.ino
@@ -164,16 +164,14 @@ static void __attribute__((unused)) SFHSS_build_data_packet()
 			//    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)
+				uint16_t val=Failsafe_data[CH_AETR[ch_offset+i]];
+				if(val==FAILSAFE_CHANNEL_HOLD)
 					ch[i]=1024;
-				else if(ch[i]==FAILSAFE_CHANNEL_NOPULSES)
+				else if(val==FAILSAFE_CHANNEL_NOPULSES)
 					ch[i]=0;
 				else
 				{ //Use channel value
-					ch[i]=(ch[i]>>1)+2560;
-					//if(IS_DISABLE_CH_MAP_off && ch_offset+i==CH3 && ch[i]<3072)		// Throttle
-					//	ch[i]+=1024;
+					ch[i] = convert_channel_16b_nolimit(CH_AETR[ch_offset+i],3472,2672,true);
 				}
 			}
 		}
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 08c276e..7769a06 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -19,7 +19,7 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		3
 #define VERSION_REVISION	2
-#define VERSION_PATCH_LEVEL	15
+#define VERSION_PATCH_LEVEL	16
 
 //******************
 // Protocols