Allow TX to request forwarding of FlySky telemetry data

Multiprotocol/AFHDS2A_a7105.ino

@@ -86,7 +86,17 @@ static void AFHDS2A_update_telemetry()
 {
 	// AA | TXID | rx_id | sensor id | sensor # | value 16 bit big endian | sensor id ......
 	// max 7 sensors per packet
+#if defined AFHDS2A_TELEMETRY
+    if (option & 0x80) {
+        // forward telemetry to TX, skip rx and tx id to save space
+        pkt[0]= TX_RSSI;
+        for(int i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
+            pkt[i-8]=packet[i];
 
+        telemetry_link=2;
+        return;
+    }
+#endif    
 	for(uint8_t sensor=0; sensor<7; sensor++)
 	{
 		uint8_t index = 9+(4*sensor);
@@ -180,7 +190,7 @@ static void AFHDS2A_build_packet(uint8_t type)
 			packet[0] = 0xaa;
 			packet[9] = 0xfd;
 			packet[10]= 0xff;
-			uint16_t val_hz=5*option+50;			// option value should be between 0 and 70 which gives a value between 50 and 400Hz
+			uint16_t val_hz=5*(option & 0x7f)+50;			// option value should be between 0 and 70 which gives a value between 50 and 400Hz
 			if(val_hz<50 || val_hz>400) val_hz=50;	// default is 50Hz
 			packet[11]= val_hz;
 			packet[12]= val_hz >> 8;

Multiprotocol/Multiprotocol.h

@@ -449,6 +449,11 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
 			FORMAT_HONTAI	0
 			FORMAT_JJRCX1	1
 			FORMAT_X5C1		2
+        sub_protocol==AFHDS2A
+        	FORMAT_IBUS+PWM 0
+        	FORMAT_IBUS+PPM 1
+        	FORMAT_SBUS+PWM 2
+        	FORMAT_SBUS+PPM 3
    Power value => 0x80	0=High/1=Low
   Stream[3]   = option_protocol;
    option_protocol value is -127..127

Multiprotocol/Multiprotocol.ino

@@ -127,7 +127,7 @@ volatile uint8_t rx_ok_buff[RXBUFFER_SIZE];
 volatile uint8_t discard_frame = 0;
 
 // Telemetry
-#define MAX_PKT 27
+#define MAX_PKT 29
 uint8_t pkt[MAX_PKT];//telemetry receiving packets
 #if defined(TELEMETRY)
 	#ifdef INVERT_TELEMETRY

Multiprotocol/Telemetry.ino

@@ -61,6 +61,15 @@ void DSM_frame()
 }
 #endif
 
+#if defined AFHDS2A_TELEMETRY
+void AFHDSA_short_frame()
+{
+	Serial_write(0xAA);					// Telemetry packet
+	for (uint8_t i = 0; i < 29; i++)	// RSSI value followed by 4*7 bytes of telemetry data
+		Serial_write(pkt[i]);
+}
+#endif
+
 void frskySendStuffed()
 {
 	Serial_write(START_STOP);
@@ -485,6 +494,13 @@ void TelemetryUpdate()
 			return;
 		}
 	#endif
+    #if defined AFHDS2A_TELEMETRY     
+        if(telemetry_link == 2 && protocol == MODE_AFHDS2A)
+            {
+                AFHDSA_short_frame();
+                telemetry_link=0;
+            }
+    #endif        
 		if(telemetry_link && protocol != MODE_FRSKYX )
 		{	// FrSkyD + Hubsan + AFHDS2A
 			frsky_link_frame();

Multiprotocol/_Config.h

@@ -106,7 +106,7 @@
 #define DSM_TELEMETRY	
 #define SPORT_TELEMETRY	
 #define HUB_TELEMETRY
-
+#define AFHDS2A_TELEMETRY
 
 /****************************/
 /*** SERIAL MODE SETTINGS ***/