M-Link telemetry forwarded to OpenTX

Multiprotocol/MLINK_cyrf6936.ino

@@ -321,6 +321,24 @@ static void __attribute__((unused)) MLINK_send_data_packet()
 	}
 #endif
 
+#ifdef MLINK_FW_TELEMETRY
+	static void __attribute__((unused)) MLINK_Send_Telemetry()
+	{
+		telemetry_counter += 2;				// TX LQI counter
+		telemetry_link = 4;
+
+		// Read TX RSSI
+		TX_RSSI = CYRF_ReadRegister(CYRF_13_RSSI)&0x1F;
+
+		if(telemetry_lost)
+		{
+			telemetry_lost = 0;
+			packet_count = 50;
+			telemetry_counter = 100;
+		}
+	}
+#endif
+
 uint16_t MLINK_callback()
 {
 	uint8_t status;
@@ -408,7 +426,7 @@ uint16_t MLINK_callback()
 			hopping_frequency_no = 0x00;
 			CYRF_ConfigRFChannel(hopping_frequency[hopping_frequency_no]);
 			CYRF_SetPower(0x38);
-			#ifdef MLINK_HUB_TELEMETRY
+			#if defined(MLINK_HUB_TELEMETRY) || defined(MLINK_FW_TELEMETRY)
 				packet_count = 0;
 				telemetry_lost = 1;
 			#endif
@@ -449,7 +467,7 @@ uint16_t MLINK_callback()
 			phase=MLINK_SEND1;
 			return 4470;
 		case MLINK_RX:
-			#ifdef MLINK_HUB_TELEMETRY
+			#if defined(MLINK_HUB_TELEMETRY) || defined(MLINK_FW_TELEMETRY)
 				//TX LQI calculation
 				packet_count++;
 				if(packet_count>=50)
@@ -470,7 +488,7 @@ uint16_t MLINK_callback()
 				if( len && len <= MLINK_PACKET_SIZE )
 				{
 					CYRF_ReadDataPacketLen(packet_in, len*2);
-					#ifdef MLINK_HUB_TELEMETRY
+					#if defined(MLINK_HUB_TELEMETRY) || defined(MLINK_FW_TELEMETRY)
 						if(len==MLINK_PACKET_SIZE)
 						{
 							for(uint8_t i=0;i<8;i++)

Multiprotocol/Multiprotocol.h

@@ -19,7 +19,7 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		3
 #define VERSION_REVISION	2
-#define VERSION_PATCH_LEVEL	35
+#define VERSION_PATCH_LEVEL	36
 
 //******************
 // Protocols
@@ -488,6 +488,7 @@ enum MultiPacketTypes
 	MULTI_TELEMETRY_AFHDS2A_AC		= 12,
 	MULTI_TELEMETRY_RX_CHANNELS		= 13,
 	MULTI_TELEMETRY_HOTT			= 14,
+	MULTI_TELEMETRY_MLINK			= 15,
 };
 
 // Macros
@@ -1234,4 +1235,10 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
    data[3] = page
    data[4-13] = data
 
+  Type 0x0F M-Link telemetry
+   length: 10
+   data[0] = TX_RSSI
+   data[1] = TX_LQI
+   data[2] = telem_type
+   data[3-9] = data
 */

Multiprotocol/Telemetry.ino

@@ -203,6 +203,17 @@ static void multi_send_status()
 	}
 }
 
+#ifdef MLINK_FW_TELEMETRY
+	void MLINK_frame()
+	{
+		multi_send_header(MULTI_TELEMETRY_MLINK, 10);
+		Serial_write(TX_RSSI);					// RSSI
+		Serial_write(TX_LQI);					// LQI
+		for (uint8_t i = 0; i < 8; i++)			// followed by 8 bytes of telemetry data
+			Serial_write(packet_in[i]);
+	}
+#endif
+
 #ifdef DSM_TELEMETRY
 	void DSM_frame()
 	{
@@ -874,6 +885,14 @@ void TelemetryUpdate()
 	#endif // SPORT_TELEMETRY
 
 	#ifdef MULTI_TELEMETRY
+		#if defined MLINK_FW_TELEMETRY
+			if(telemetry_link && protocol == PROTO_MLINK)
+			{
+				MLINK_frame();
+				telemetry_link=0;
+				return;
+			}
+		#endif
 		#if defined DSM_TELEMETRY
 			if(telemetry_link && protocol == PROTO_DSM)
 			{	// DSM

Multiprotocol/Validate.h

@@ -359,6 +359,8 @@
 	#undef DSM_FWD_PGM
 	#undef WFLY2_HUB_TELEMETRY
 	#undef LOLI_HUB_TELEMETRY
+	#undef MLINK_HUB_TELEMETRY
+	#undef MLINK_FW_TELEMETRY
 #else
 	#if not defined(SCANNER_CC2500_INO) || not defined(SCANNER_TELEMETRY)
 		#undef SCANNER_TELEMETRY
@@ -423,6 +425,7 @@
 	#endif
 	#if not defined(MLINK_CYRF6936_INO)
 		#undef MLINK_HUB_TELEMETRY
+		#undef MLINK_FW_TELEMETRY
 	#endif
 	#if not defined(HOTT_CC2500_INO)
 		#undef HOTT_FW_TELEMETRY
@@ -437,13 +440,17 @@
 		//protocols using FRSKYD user frames
 		#undef HUB_TELEMETRY 
 	#endif
-	#if not defined(HOTT_FW_TELEMETRY) && not defined(DSM_TELEMETRY) && not defined(SPORT_TELEMETRY) && not defined(HUB_TELEMETRY) && not defined(HUBSAN_HUB_TELEMETRY) && not defined(BUGS_HUB_TELEMETRY) && not defined(NCC1701_HUB_TELEMETRY) && not defined(BAYANG_HUB_TELEMETRY) && not defined(CABELL_HUB_TELEMETRY) && not defined(RLINK_HUB_TELEMETRY) && not defined(AFHDS2A_HUB_TELEMETRY) && not defined(AFHDS2A_FW_TELEMETRY) && not defined(MULTI_TELEMETRY) && not defined(MULTI_STATUS) && not defined(HITEC_HUB_TELEMETRY) && not defined(HITEC_FW_TELEMETRY) && not defined(SCANNER_TELEMETRY) && not defined(FRSKY_RX_TELEMETRY) && not defined(AFHDS2A_RX_TELEMETRY) && not defined(BAYANG_RX_TELEMETRY) && not defined(DEVO_HUB_TELEMETRY) && not defined(PROPEL_HUB_TELEMETRY) && not defined(OMP_HUB_TELEMETRY) && not defined(WFLY2_HUB_TELEMETRY) && not defined(LOLI_HUB_TELEMETRY) && not defined(MLINK_HUB_TELEMETRY)
+	#if not defined(HOTT_FW_TELEMETRY) && not defined(DSM_TELEMETRY) && not defined(SPORT_TELEMETRY) && not defined(HUB_TELEMETRY) && not defined(HUBSAN_HUB_TELEMETRY) && not defined(BUGS_HUB_TELEMETRY) && not defined(NCC1701_HUB_TELEMETRY) && not defined(BAYANG_HUB_TELEMETRY) && not defined(CABELL_HUB_TELEMETRY) && not defined(RLINK_HUB_TELEMETRY) && not defined(AFHDS2A_HUB_TELEMETRY) && not defined(AFHDS2A_FW_TELEMETRY) && not defined(MULTI_TELEMETRY) && not defined(MULTI_STATUS) && not defined(HITEC_HUB_TELEMETRY) && not defined(HITEC_FW_TELEMETRY) && not defined(SCANNER_TELEMETRY) && not defined(FRSKY_RX_TELEMETRY) && not defined(AFHDS2A_RX_TELEMETRY) && not defined(BAYANG_RX_TELEMETRY) && not defined(DEVO_HUB_TELEMETRY) && not defined(PROPEL_HUB_TELEMETRY) && not defined(OMP_HUB_TELEMETRY) && not defined(WFLY2_HUB_TELEMETRY) && not defined(LOLI_HUB_TELEMETRY) && not defined(MLINK_HUB_TELEMETRY) && not defined(MLINK_FW_TELEMETRY)
 		#undef TELEMETRY
 		#undef INVERT_TELEMETRY
 		#undef MULTI_TELEMETRY
 	#endif
 #endif
 
+#if defined(MLINK_HUB_TELEMETRY) && defined(MLINK_FW_TELEMETRY)
+	#error "You must select MLINK_HUB_TELEMETRY or MLINK_FW_TELEMETRY, not both."
+#endif
+
 #ifdef SPORT_TELEMETRY
 	#define SPORT_SEND
 #endif

Multiprotocol/_Config.h

@@ -327,7 +327,8 @@
 #define RLINK_HUB_TELEMETRY			// Use FrSkyD Hub format to send telemetry to TX
 #define WFLY2_HUB_TELEMETRY			// Use FrSkyD Hub format to send telemetry to TX
 #define LOLI_HUB_TELEMETRY			// Use FrSkyD Hub format to send telemetry to TX
-#define MLINK_HUB_TELEMETRY			// Use FrSkyD Hub format to send telemetry to TX
+//#define MLINK_HUB_TELEMETRY			// Use FrSkyD Hub format to send telemetry to TX
+#define MLINK_FW_TELEMETRY			// Forward received telemetry packet directly to TX to be decoded by erskyTX and OpenTX
 //#define HITEC_HUB_TELEMETRY		// Use FrSkyD Hub format to send basic telemetry to the radios which can decode it like er9x, erskyTX and OpenTX
 #define HITEC_FW_TELEMETRY			// Forward received telemetry packets to be decoded by erskyTX and OpenTX
 #define SCANNER_TELEMETRY			// Forward spectrum scanner data to TX