From f9e6b305509108f1088a61cea12c36f62e71b2b6 Mon Sep 17 00:00:00 2001
From: midelic <midelic@gmail.com>
Date: Sat, 4 Nov 2017 20:40:20 +0100
Subject: [PATCH] Fix AFHDS2A failsafe problem

---
 Multiprotocol/AFHDS2A_a7105.ino | 22 ++++++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino
index 158142d..2eacf45 100644
--- a/Multiprotocol/AFHDS2A_a7105.ino
+++ b/Multiprotocol/AFHDS2A_a7105.ino
@@ -179,12 +179,26 @@ static void AFHDS2A_build_packet(uint8_t type)
 			packet[0] = 0x56;
 			for(uint8_t ch=0; ch<14; ch++)
 			{
-				/*if((Model.limits[ch].flags & CH_FAILSAFE_EN))
+#ifdef AFHDS2A_FAILSAFE
+				int8_t failsafe = AFHDS2AFailsafe[ch];
+				//
+				if(failsafe != -1)
 				{
-					packet[9 + ch*2] = Servo_data[CH_AETR[ch]] & 0xff;
-					packet[10+ ch*2] = (Servo_data[CH_AETR[ch]] >> 8) & 0xff;
+					//
+					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*/
+				else
+#endif
 				{
 					packet[9 + ch*2] = 0xff;
 					packet[10+ ch*2] = 0xff;