Merge pull request #35 from schwabe/multi_telemetry

Multi telemetry

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")
 
@@ -523,3 +535,63 @@ Serial: 100000 Baud 8e2      _ xxxx xxxx p --
 	2047	+125%
    Channels bits are concatenated to fit in 22 bytes like in SBUS protocol
 */
+/*
+  Multiprotocol telemetry definition
+
+  Serial: 100000 Baud 8e2 (same as input)
+
+  TLV Protocol (type, length, value), allows a TX to ignore unknown messages
+
+  Format: header (4 byte) + data (variable)
+
+   [0] = 'M' (0x4d)
+   [1] = 'P' (0x50)
+
+
+   The first byte is deliberatly chosen to be different from other telemetry protocols
+   (e.g. 0xAA for DSM/Multi, 0xAA for FlySky and 0x7e for Frsky) to allow a TX to detect
+   the telemetry format of older versions
+
+   [2] Type (see below)
+   [3] Length (excluding the 4 header bytes)
+
+   [4-xx] data
+
+
+Type = 0x01 Multimodule Status:
+   [4] Flags
+   0x01 = Input signal detected
+   0x02 = Serial mode enabled
+   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
+
+
+   more information can be added by specifing a longer length of the type, the TX will just ignore these bytes
+
+
+Type 0x02 Frksy S.port telemetry
+Type 0x03 Frsky Hub telemetry
+
+	*No* usual frsky byte stuffing and without start/stop byte (0x7e)
+
+
+Type 0x04 Spektrum telemetry data
+   data[0] RSSI
+   data[1-15] telemetry data
+
+Type 0x05 DSM bind data
+	data[0-16] DSM bind data
+
+    technically DSM bind data is only 10 bytes but multi send 16
+    like with telemtry, check length field)
+
+Type 0x06 Flysky AFHDS2 telemetry data
+   length: 29
+   data[0] = RSSI value
+   data[1-28] 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

@@ -17,8 +17,13 @@
 //**************************
 #if defined TELEMETRY
 
+#if defined MULTI_TELEMETRY
+	#define MULTI_TIME 250 //in ms
+	uint32_t lastMulti = 0;
+#endif
+
 #if defined SPORT_TELEMETRY	
-	#define SPORT_TIME 12000
+    #define SPORT_TIME 12000  //12ms
 	#define FRSKY_SPORT_PACKET_SIZE   8
 	uint32_t last = 0;
 	uint8_t sport_counter=0;
@@ -52,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
@@ -60,11 +120,16 @@ void DSM_frame()
 		Serial_write(pkt[i]);
 }
 #endif
+#endif
 
 #ifdef AFHDS2A_FW_TELEMETRY
 void AFHDSA_short_frame()
 {
+#if defined MULTI_TELEMETRY
+    multi_send_header(MULTI_TELEMETRY_AFHDS2A, 29);
+#else
     Serial_write(0xAA);                    // Telemetry packet
+#endif
 	for (uint8_t i = 0; i < 29; i++)	// RSSI value followed by 4*7 bytes of telemetry data
 		Serial_write(pkt[i]);
 }
@@ -157,7 +222,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
@@ -223,7 +292,11 @@ void frsky_user_frame()
 		if(!indx)
 			return;
 		frame[1] = indx;
+#if defined MULTI_TELEMETRY
+        multi_send_frskyhub();
+#else
 		frskySendStuffed();
+#endif
 	}
 	else
 		pass=0;
@@ -294,6 +367,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;
@@ -320,10 +415,13 @@ void sportSend(uint8_t *p)
 		}
 	}
 }
+#endif
 
 void sportIdle()
 {
+#if !defined MULTI_TELEMETRY
 	Serial_write(START_STOP);
+#endif
 }	
 
 void sportSendFrame()
@@ -468,6 +566,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)

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 ***/