From f105d7c7eb4894056c757d60cbcff0426531a9aa Mon Sep 17 00:00:00 2001
From: Pascal Langer <planger@mathworks.com>
Date: Wed, 8 Aug 2018 00:30:19 +0200
Subject: [PATCH] Hitec full telemetry

---
 Multiprotocol/Hitec_cc2500.ino |  9 ++++++---
 Multiprotocol/Multiprotocol.h  | 12 ++++++++----
 Multiprotocol/Telemetry.ino    | 25 +++++++++++++++++++++++--
 Multiprotocol/_Config.h        |  4 ++--
 4 files changed, 39 insertions(+), 11 deletions(-)

diff --git a/Multiprotocol/Hitec_cc2500.ino b/Multiprotocol/Hitec_cc2500.ino
index 50530ea..df0f179 100644
--- a/Multiprotocol/Hitec_cc2500.ino
+++ b/Multiprotocol/Hitec_cc2500.ino
@@ -348,11 +348,11 @@ uint16_t ReadHITEC()
 								telemetry_link=1;			// telemetry hub available
 							#elif defined(HITEC_FW_TELEMETRY)
 								// 8 bytes telemetry packets => see at the end of this file how to fully decode it
+								pkt[0]=pkt[13];				// TX RSSI
+								pkt[1]=pkt[14]&0x7F;		// LQI
 								uint8_t offset=pkt[5]==0?1:0;
 								for(uint8_t i=5;i < 11; i++)
-									pkt[i-5]=pkt[i+offset];		// frame number followed by 5 bytes of data
-								pkt[6]=pkt[13];				// TX RSSI
-								pkt[7]=pkt[14]&0x7F;		// LQI
+									pkt[i-3]=pkt[i+offset];	// frame number followed by 5 bytes of data
 								telemetry_link=2;			// telemetry forward available
 							#endif
 						}
@@ -384,6 +384,9 @@ uint16_t initHITEC()
 		rx_tx_addr[3]=0x6A;
 		memcpy((void *)hopping_frequency,(void *)"\x00\x3A\x4A\x32\x0C\x58\x2A\x10\x26\x20\x08\x60\x68\x70\x78\x80\x88\x56\x5E\x66\x6E",HITEC_NUM_FREQUENCE);
 	#endif
+	#if defined(HITEC_HUB_TELEMETRY)
+		init_frskyd_link_telemetry();
+	#endif
 	phase = HITEC_START;
 	return 10000;
 }
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 8f98b45..be46f62 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -19,7 +19,7 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		2
 #define VERSION_REVISION	0
-#define VERSION_PATCH_LEVEL	28
+#define VERSION_PATCH_LEVEL	29
 
 //******************
 // Protocols
@@ -777,7 +777,7 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
 
 
   Type 0x04 Spektrum telemetry data
-   data[0] RSSI
+   data[0] TX RSSI
    data[1-15] telemetry data
 
   Type 0x05 DSM bind data
@@ -792,7 +792,11 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
    data[1-28] telemetry data
 
   Type 0x0A Hitec telemetry data
-   length: 12
-   data[0-10] telemetry data
+   length: 8
+   data[0] = TX RSSI value
+   data[1] = TX LQI value
+   data[2] = frame number
+   data[3-7] telemetry data
+   Full description at the bottom of Hitec_cc2500.ino
 
 */
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 1a6b723..06ad339 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -183,6 +183,19 @@ static void multi_send_status()
 	}
 #endif
 
+#ifdef HITEC_FW_TELEMETRY
+	void HITEC_short_frame()
+	{
+		#if defined MULTI_TELEMETRY
+			multi_send_header(MULTI_TELEMETRY_HITEC, 8);
+		#else
+			Serial_write(0xAA);					// Telemetry packet
+		#endif
+		for (uint8_t i = 0; i < 8; i++)			// TX RSSI and TX LQI values followed by frame number and 5 bytes of telemetry data
+			Serial_write(pkt[i]);
+	}
+#endif
+
 #ifdef MULTI_TELEMETRY
 static void multi_send_frskyhub()
 {
@@ -357,7 +370,7 @@ void frsky_link_frame()
 		if (protocol==PROTO_HUBSAN||protocol==PROTO_AFHDS2A||protocol==PROTO_BAYANG||protocol==PROTO_CABELL||protocol==PROTO_HITEC)
 		{	
 			frame[1] = v_lipo1;
-			frame[2] = v_lipo2;			
+			frame[2] = v_lipo2;
 			frame[3] = RX_RSSI;
 			telemetry_link=0;
 		}
@@ -974,9 +987,17 @@ void TelemetryUpdate()
 			return;
 		}
 	#endif
+	#if defined HITEC_FW_TELEMETRY
+		if(telemetry_link == 2 && protocol == PROTO_HITEC)
+		{
+			HITEC_short_frame();
+			telemetry_link=0;
+			return;
+		}
+	#endif
 
 		if((telemetry_link & 1 )&& protocol != PROTO_FRSKYX)
-		{	// FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell
+		{	// FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec
 			frsky_link_frame();
 			return;
 		}
diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h
index 8fef94a..6b836b6 100644
--- a/Multiprotocol/_Config.h
+++ b/Multiprotocol/_Config.h
@@ -248,8 +248,8 @@
 #define BAYANG_HUB_TELEMETRY		// Use FrSkyD Hub format to send telemetry to TX
 #define HUBSAN_HUB_TELEMETRY		// Use FrSkyD Hub format to send telemetry to TX
 #define CABELL_HUB_TELEMETRY		// Use FrSkyD Hub format to send telemetry to TX
-#define HITEC_HUB_TELEMETRY			// Use FrSkyD Hub format to send telemetry to TX
-//#define HITEC_FW_TELEMETRY			// Forward received telemetry packet directly to TX to be decoded
+#define HITEC_HUB_TELEMETRY			// Use FrSkyD Hub format to send telemetry to er9x and ersky9x
+//#define HITEC_FW_TELEMETRY		// Under development: Forward received telemetry packets directly to be decoded by ersky9x or OpenTX
 
 //SPORT_POLLING is an implementation of the same polling routine as XJT module for sport telemetry bidirectional communication.
 //This is useful for passing sport control frames from TX to RX(ex: changing Betaflight PID or VTX channels on the fly using LUA scripts with OpentX).