From 47b4409f3e648f2801d8a9df76e385f2d3d4ef3a Mon Sep 17 00:00:00 2001
From: pascallanger <planger@mathworks.com>
Date: Thu, 2 Feb 2017 14:30:56 +0100
Subject: [PATCH] FrSkyD telemetry

---
 Multiprotocol/Telemetry.ino | 16 +++++++++-------
 1 file changed, 9 insertions(+), 7 deletions(-)

diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index b7705c4..5e7c582 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -175,8 +175,9 @@ void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
 		TX_LQI = pkt[len-1]&0x7F;
 		for (uint8_t i=3;i<len-2;i++)
 			pktt[i]=pkt[i];								// Buffer telemetry values to be sent 
-		if(pktt[6]>0 && pktt[6]<=10) // && (pktt[7]&0x1F) == telemetry_counter )
-			telemetry_counter=(telemetry_counter+1)%32;	// Request next telemetry frame
+		uint8_t next_sequence = (telemetry_counter+1)%32;
+		if((pktt[6]>0 && pktt[6]<=10) && ( pktt[7] & 0x1F ) == next_sequence )
+			telemetry_counter = next_sequence ;			// Request next telemetry frame
 		else
 			pktt[6]=0; 									// Discard packet
 		//
@@ -258,6 +259,7 @@ void frsky_user_frame()
 		else
 		{
 			frame[1]=pktt[6];			// packet size
+			pktt[6]=0;
 			telemetry_link=0;			// only 1 packet or processing second packet
 		}
 		frame[2] = pktt[7];
@@ -581,11 +583,6 @@ void TelemetryUpdate()
 			return;
 		}
     #endif        
-		if(telemetry_link == 1 && protocol != MODE_FRSKYX)
-		{	// FrSkyD + Hubsan + AFHDS2A + Bayang
-			frsky_link_frame();
-			return;
-		}
 	#if defined HUB_TELEMETRY
 		if(telemetry_link > 1 && protocol == MODE_FRSKYD)
 		{	// FrSkyD
@@ -593,6 +590,11 @@ void TelemetryUpdate()
 			return;
 		}
 	#endif
+	if(telemetry_link == 1 && protocol != MODE_FRSKYX)
+	{	// FrSkyD + Hubsan + AFHDS2A + Bayang
+		frsky_link_frame();
+		return;
+	}
 }