From 3c76ce9f39c3d4e48a2a7d9482fc152533a189aa Mon Sep 17 00:00:00 2001
From: Pascal Langer <planger@mathworks.com>
Date: Sat, 14 Sep 2019 16:27:07 +0200
Subject: [PATCH] AFHDS2A telemetry AA and AC

---
 Multiprotocol/AFHDS2A_a7105.ino |  5 +++--
 Multiprotocol/Multiprotocol.h   | 13 ++++++++++---
 Multiprotocol/Multiprotocol.ino |  2 +-
 Multiprotocol/Telemetry.ino     |  4 ++--
 4 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino
index 8be95f2..df73b87 100644
--- a/Multiprotocol/AFHDS2A_a7105.ino
+++ b/Multiprotocol/AFHDS2A_a7105.ino
@@ -89,12 +89,13 @@ static void AFHDS2A_update_telemetry()
 		if (option & 0x80)
 		{// forward 0xAA and 0xAC telemetry to TX, skip rx and tx id to save space
 			pkt[0]= TX_RSSI;
-			debug("T=");
-			for(int i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
+			debug("T(%02X)=",packet[0]);
+			for(uint8_t i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
 			{
 				pkt[i-8]=packet[i];
 				debug(" %02X",packet[i]);
 			}
+			pkt[29]=packet[0];	// 0xAA Normal telemetry, 0xAC Extended telemetry
 			telemetry_link=2;
 			debugln("");
 			return;
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 79f0208..8e8a631 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -19,7 +19,7 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		2
 #define VERSION_REVISION	1
-#define VERSION_PATCH_LEVEL	76
+#define VERSION_PATCH_LEVEL	77
 
 //******************
 // Protocols
@@ -320,6 +320,7 @@ enum MultiPacketTypes
 	MULTI_TELEMETRY_SPORT_POLLING	= 9,
 	MULTI_TELEMETRY_HITEC			= 10,
 	MULTI_TELEMETRY_SCANNER			= 11,
+	MULTI_TELEMETRY_AFHDS2A_AC		= 12,
 };
 
 // Macros
@@ -875,7 +876,7 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
     technically DSM bind data is only 10 bytes but multi sends 16
     like with telemtery, check length field)
 
-  Type 0x06 Flysky AFHDS2 telemetry data
+  Type 0x06 Flysky AFHDS2 telemetry data type 0xAA
    length: 29
    data[0] = RSSI value
    data[1-28] telemetry data
@@ -888,8 +889,14 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
    data[3-7] telemetry data
    Full description at the bottom of Hitec_cc2500.ino
 
-   Type 0x0B Spectrum Scanner telemetry data
+  Type 0x0B Spectrum Scanner telemetry data
    length: 6
    data[0] = start channel (2400 + x*0.333 Mhz)
    data[1-5] power levels
+
+  Type 0x0C Flysky AFHDS2 telemetry data type 0xAC
+   length: 29
+   data[0] = RSSI value
+   data[1-28] telemetry data
+
 */
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index 2f05d47..8c33535 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -171,7 +171,7 @@ volatile uint8_t rx_ok_buff[RXBUFFER_SIZE];
 volatile uint8_t discard_frame = 0;
 
 // Telemetry
-#define MAX_PKT 29
+#define MAX_PKT 30
 uint8_t pkt[MAX_PKT];//telemetry receiving packets
 #if defined(TELEMETRY)
 	#ifdef INVERT_TELEMETRY
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 420b225..0a4023e 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -188,9 +188,9 @@ static void multi_send_status()
 	void AFHDSA_short_frame()
 	{
 		#if defined MULTI_TELEMETRY
-			multi_send_header(MULTI_TELEMETRY_AFHDS2A, 29);
+			multi_send_header(pkt[29]==0xAA?MULTI_TELEMETRY_AFHDS2A,MULTI_TELEMETRY_AFHDS2A_AC, 29);
 		#else
-			Serial_write(0xAA);						// Telemetry packet
+			Serial_write(pkt[29]);					// Telemetry packet 0xAA or 0xAC
 		#endif
 		for (uint8_t i = 0; i < 29; i++)			// RSSI value followed by 4*7 bytes of telemetry data
 			Serial_write(pkt[i]);