Implement Multiprotocol Telemetry

This allows the multi module to tell the TX software (e.g. OpenTX) which telemetry protocol is in use. Also Status of the module and signaling binding/invalid protocol

Multiprotocol/AFHDS2A_a7105.ino

@@ -1,4 +1,4 @@
-/*
+ /*
  This project is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or

Multiprotocol/Multiprotocol.h

@@ -183,6 +183,18 @@ struct PPM_Parameters
 	uint8_t option;
 };
 
+// Telemetry
+
+enum MultiPacketTypes {
+    MULTI_TELEMETRY_STATUS  = 1,
+    MULTI_TELEMETRY_SPORT   = 2,
+    MULTI_TELEMETRY_HUB     = 3,
+    MULTI_TELEMETRY_DSM     = 4,
+    MULTI_TELEMETRY_DSMBIND = 5,
+    MULTI_TELEMETRY_AFHDS2A = 6,
+};
+
+
 // Macros
 #define NOP() __asm__ __volatile__("nop")
 
@@ -548,13 +560,15 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
 
 Type = 0x01 Multimodule Status:
    [4] Flags
-
    0x01 = Input signal detected
    0x02 = Serial mode enabled
-   0x04 = protocol is invalid
+   0x04 = protocol is valid
    0x08 = module is in binding mode
+   [5] major
+   [6] mior
+   [7-8] patchlevel
+   version of multi code, should be displayed as major.minor.patchlevel
 
-   [5-8] uint32  - version of multi code
 
    more information can be added by specifing a longer length of the type, the TX will just ignore these bytes
 
@@ -562,7 +576,7 @@ Type = 0x01 Multimodule Status:
 Type 0x02 Frksy S.port telemetry
 Type 0x03 Frsky Hub telemetry
 
-	*No* usual frsky byte stuffing
+	*No* usual frsky byte stuffing and without start/stop byte (0x7e)
 
 
 Type 0x04 Spektrum telemetry data

Multiprotocol/Multiprotocol.ino

@@ -490,7 +490,9 @@ void Update_All()
 	#endif //ENABLE_PPM
 	update_channels_aux();
 	#if defined(TELEMETRY)
+		#if !defined(MULTI_TELEMETRY)
 		if((protocol==MODE_FRSKYD) || (protocol==MODE_BAYANG) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) )
+		#endif
 			TelemetryUpdate();
 	#endif
 	update_led_status();

Multiprotocol/Telemetry.ino

@@ -18,7 +18,7 @@
 #if defined TELEMETRY
 
 #if defined MULTI_TELEMETRY
-	#define MULTI_TIME 1000*1000*1000 // 1s
+	#define MULTI_TIME 250 //in ms
 	uint32_t lastMulti = 0;
 #endif
 
@@ -57,7 +57,62 @@ uint8_t frame[18];
 	} SerialControl ;
 #endif
 
+#ifdef MULTI_TELEMETRY
+static void multi_send_header(uint8_t type, uint8_t len)
+{
+    Serial_write('M');
+    Serial_write('P');
+    Serial_write(type);
+    Serial_write(len);
+}
+
+static void multi_send_frskyhub()
+{
+    multi_send_header(MULTI_TELEMETRY_HUB, 9);
+    for (uint8_t i = 0; i < 9; i++)
+        Serial_write(frame[i]);
+}
+
+static void multi_send_status()
+{
+    multi_send_header(MULTI_TELEMETRY_STATUS, 5);
+
+    // Build flags
+    uint8_t flags;
+    if (millis()-last_signal<70)
+        flags |= 0x01;
+    if (mode_select==MODE_SERIAL)
+        flags |= 0x02;
+    if (remote_callback != 0)
+        flags |= 0x04;
+    if (!IS_BIND_DONE_on)
+        flags |= 0x08;
+    Serial_write(flags);
+
+    // Version number: 1.15.0
+    Serial_write(1);
+    Serial_write(15);
+    Serial_write(0);
+    Serial_write(0);
+}
+#endif
+
 #ifdef DSM_TELEMETRY
+#if defined MULTI_TELEMETRY
+void DSM_frame()
+{
+    if (pkt[0] == 0x80) {
+        multi_send_header(MULTI_TELEMETRY_DSMBIND, 10);
+	for (uint8_t i = 1; i < 11; i++) // 10 byte of DSM bind response
+            Serial_write(pkt[i]);
+
+    } else {
+        multi_send_header(MULTI_TELEMETRY_DSM, 17);
+        for (uint8_t i = 0; i < 29; i++)	// RSSI value followed by 4*7 bytes of telemetry data
+            Serial_write(pkt[i]);
+    }
+}
+#else
 void DSM_frame()
 {
 	Serial_write(0xAA);					// Telemetry packet
@@ -65,11 +120,12 @@ void DSM_frame()
 		Serial_write(pkt[i]);
 }
 #endif
+#endif
 
 #ifdef AFHDS2A_FW_TELEMETRY
 void AFHDSA_short_frame()
 {
-	Serial_write(0xAA);					// Telemetry packet
+    multi_send_header(MULTI_TELEMETRY_AFHDS2A, 29);
 	for (uint8_t i = 0; i < 29; i++)	// RSSI value followed by 4*7 bytes of telemetry data
 		Serial_write(pkt[i]);
 }
@@ -162,7 +218,11 @@ void frsky_link_frame()
 			frame[4] = TX_RSSI;
 		}
 	frame[5] = frame[6] = frame[7] = frame[8] = 0;
+#if defined MULTI_TELEMETRY
+    multi_send_frskyhub();
+#else
 	frskySendStuffed();
+#endif
 }
 
 #if defined HUB_TELEMETRY
@@ -228,7 +288,11 @@ void frsky_user_frame()
 		if(!indx)
 			return;
 		frame[1] = indx;
+#if defined MULTI_TELEMETRY
+        multi_send_frskyhub();
+#else
 		frskySendStuffed();
+#endif
 	}
 	else
 		pass=0;
@@ -299,6 +363,28 @@ pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
 0x34	0x0A	0xC3	0x56	0xF3
 				
 		*/
+#ifdef MULTI_TELEMETRY
+void sportSend(uint8_t *p)
+{
+    multi_send_header(MULTI_TELEMETRY_SPORT, 9);
+	uint16_t crc_s = 0;
+	Serial_write(p[0]) ;
+	for (uint8_t i = 1; i < 9; i++)
+	{
+		if (i == 8)
+			p[i] = 0xff - crc_s;
+			Serial_write(p[i]);
+
+		if (i>0)
+		{
+			crc_s += p[i]; //0-1FF
+			crc_s += crc_s >> 8; //0-100
+			crc_s &= 0x00ff;
+		}
+	}
+}
+
+#else
 void sportSend(uint8_t *p)
 {
 	uint16_t crc_s = 0;
@@ -325,10 +411,13 @@ void sportSend(uint8_t *p)
 		}
 	}
 }
+#endif
 
 void sportIdle()
 {
+#if !defined MULTI_TELEMETRY
 	Serial_write(START_STOP);
+#endif
 }	
 
 void sportSendFrame()
@@ -473,6 +562,16 @@ void TelemetryUpdate()
 			return ;
 		}
 	#endif
+    #if defined MULTI_TELEMETRY
+        {
+            uint32_t now = millis();
+            if ((now - lastMulti) > MULTI_TIME) {
+                multi_send_status();
+                lastMulti = now;
+                return;
+            }
+        }
+	#endif
 	 
 	#if defined SPORT_TELEMETRY
 		if (protocol==MODE_FRSKYX)
@@ -529,13 +628,6 @@ void TelemetryUpdate()
 			return;
 		}
 	#endif
-    #if defined MULTI_TELEMETRY
-        {
-            uint32_t now = micros();
-            if ((now - lastMulti) > MULTI_TIME) {
-                sendMultiStatus();
-            }
-        }
 }
 
 

Multiprotocol/_Config.h

@@ -119,7 +119,7 @@
 #define AFHDS2A_HUB_TELEMETRY		// Use FrSkyD Hub format to send telemetry to TX
 #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 MULTI_TELEMETRY				// Send also Multi status and wrap other telemetry to allow TX to autodetect the format
 
 /****************************/
 /*** SERIAL MODE SETTINGS ***/