DEVO: full telemetry

Multiprotocol/Devo_cyrf6936.ino

@@ -163,6 +163,35 @@ static void __attribute__((unused)) DEVO_build_data_pkt()
 }
 
 #if defined DEVO_HUB_TELEMETRY
+static uint32_t __attribute__((unused)) DEVO_text_to_int(uint8_t *ptr, uint8_t len)
+{
+	uint32_t value = 0;
+	for(uint8_t i = 0; i < len; i++)
+		value = value * 10 + (ptr[i] - '0');
+	return value;
+}
+
+static void __attribute__((unused)) DEVO_float_to_ints(uint8_t *ptr, uint16_t *value, uint16_t *decimal)
+{
+	bool seen_decimal = false;
+	*value = 0;
+	*decimal = 0;
+	for(uint8_t i = 0; i < 7; i++)
+	{
+		if(ptr[i] == '.')
+		{
+			seen_decimal = true;
+			continue;
+		}
+		if(ptr[i] == 0)
+			break;
+		if(seen_decimal)
+			*decimal = *decimal * 10 + (ptr[i] - '0');
+		else
+			*value = *value * 10 + (ptr[i] - '0');
+	}
+}
+
 static void __attribute__((unused)) DEVO_parse_telemetry_packet()
 {
 	DEVO_scramble_pkt(); //This will unscramble the packet
@@ -177,14 +206,55 @@ static void __attribute__((unused)) DEVO_parse_telemetry_packet()
 	RX_RSSI = TX_RSSI;
 	telemetry_link = 1;
 
-	//TODO: FW telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232
-
 	debug("P[0]=%02X",packet[0]);
 	
-	if (packet[0] == 0x30)	// Volt packet
+	//Telemetry https://github.com/DeviationTX/deviation/blob/5efb6a28bea697af9a61b5a0ed2528cc8d203f90/src/protocol/devo_cyrf6936.c#L232
+	uint16_t val, dec;
+	switch(packet[0])
 	{
-		v_lipo1 = packet[1] << 1;
+		case 0x30:	// Volt and RPM packet
-		v_lipo2 = packet[3] << 1;
+			v_lipo1 = packet[1] << 1;
+			v_lipo2 = packet[3] << 1;
+			val = packet[7] * 120; //In RPM
+			frsky_send_user_frame(0x03, val, val>>8);														// RPM
+			break;
+		case 0x31:	// Temperature packet
+			if(packet[1]!=0xff)
+				frsky_send_user_frame(0x02, packet[1], 0x00);												// temp1
+			if(packet[2]!=0xff)
+				frsky_send_user_frame(0x05, packet[2], 0x00);												// temp2
+			break;
+		// GPS Data
+		case 0x32: // Longitude
+			val = DEVO_text_to_int(&packet[1], 3)*100 + DEVO_text_to_int(&packet[4], 2);					// dddmm
+			frsky_send_user_frame(0x12  , val, val>>8);
+			val = DEVO_text_to_int(&packet[7], 4);															// .mmmm
+			frsky_send_user_frame(0x12+8, val, val>>8);
+			frsky_send_user_frame(0x1A+8, packet[11], 0x00);												// 'E'/'W'
+			break;
+		case 0x33: // Latitude
+			val = DEVO_text_to_int(&packet[1], 2)*100 + DEVO_text_to_int(&packet[3], 2);					// ddmm
+			frsky_send_user_frame(0x13  , val, val>>8);
+			val = DEVO_text_to_int(&packet[6], 4);															// .mmmm
+			frsky_send_user_frame(0x13+8, val, val>>8);
+			frsky_send_user_frame(0x1B+8, packet[10], 0x00);												// 'N'/'S'
+			break;
+		case 0x34: // Altitude
+			DEVO_float_to_ints(&packet[1], &val, &dec);
+			frsky_send_user_frame(0x10, val, val>>8);
+			frsky_send_user_frame(0x21, dec, dec>>8);
+			break;
+		case 0x35: // Speed
+			DEVO_float_to_ints(&packet[7], &val, &dec);
+			frsky_send_user_frame(0x11  , val, val>>8);
+			frsky_send_user_frame(0x11+8, dec, dec>>8);
+			break;
+		case 0x36: // Time
+			frsky_send_user_frame(0x15, DEVO_text_to_int(&packet[9], 2), DEVO_text_to_int(&packet[7], 2));	// month, day
+			frsky_send_user_frame(0x16, DEVO_text_to_int(&packet[11], 2), 0x00);							// year
+			frsky_send_user_frame(0x17, DEVO_text_to_int(&packet[1], 2), DEVO_text_to_int(&packet[3], 2));	// hour, min
+			frsky_send_user_frame(0x18, DEVO_text_to_int(&packet[5], 2), 0x00);								// second
+			break;
 	}
 }
 #endif

Multiprotocol/Multi_Protos.ino

@@ -173,6 +173,9 @@ enum
 
 const mm_protocol_definition multi_protocols[] = {
 // Protocol number, Protocol String, Sub_protocol strings, Number of sub_protocols, Option type, Failsafe, ChMap, RF switch, Init, Callback
+	#if defined(MULTI_CONFIG_INO)
+		{PROTO_CONFIG,     STR_CONFIG,    NO_SUBTYPE,            0, OPTION_NONE,    0, 0, 0,         CONFIG_init,     CONFIG_callback     },
+	#endif
 	#if defined(ASSAN_NRF24L01_INO)
 		{PROTO_ASSAN,      STR_ASSAN,     NO_SUBTYPE,            0, OPTION_NONE,    0, 0, SW_NRF,    ASSAN_init,      ASSAN_callback      },
 	#endif

Multiprotocol/Multiprotocol.h

@@ -19,14 +19,16 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		3
 #define VERSION_REVISION	2
-#define VERSION_PATCH_LEVEL	51
+#define VERSION_PATCH_LEVEL	52
+
+#define MODE_SERIAL 0
 
 //******************
 // Protocols
 //******************
 enum PROTOCOLS
 {
-	MODE_SERIAL		= 0,	// Serial commands
+	PROTO_CONFIG	= 0,	// Module config
 	PROTO_FLYSKY 	= 1,	// =>A7105
 	PROTO_HUBSAN	= 2,	// =>A7105
 	PROTO_FRSKYD	= 3,	// =>CC2500
@@ -489,6 +491,7 @@ enum MultiPacketTypes
 	MULTI_TELEMETRY_RX_CHANNELS		= 13,
 	MULTI_TELEMETRY_HOTT			= 14,
 	MULTI_TELEMETRY_MLINK			= 15,
+	MULTI_TELEMETRY_CONFIG			= 16,
 };
 
 // Macros

Multiprotocol/Multiprotocol.ino

@@ -257,6 +257,10 @@ uint8_t packet_in[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets
 		uint8_t DSM_SerialRX_val[7];
 		bool DSM_SerialRX=false;
 	#endif
+	#ifdef MULTI_CONFIG_INO
+		uint8_t CONFIG_SerialRX_val[7];
+		bool CONFIG_SerialRX=false;
+	#endif
 #endif // TELEMETRY
 
 uint8_t multi_protocols_index=0xFF;
@@ -1503,6 +1507,13 @@ void update_serial_data()
 				DSM_SerialRX=true;
 			}
 		#endif
+		#ifdef MULTI_CONFIG_INO
+			if(protocol==PROTO_CONFIG && rx_len==27+7)
+			{//Protocol waiting for 7 bytes
+				memcpy(CONFIG_SerialRX_val, (const void *)&rx_ok_buff[27],7);
+				CONFIG_SerialRX=true;
+			}
+		#endif
 	}
 
 	RX_DONOTUPDATE_off;

Multiprotocol/Telemetry.ino

@@ -53,6 +53,14 @@ uint8_t RetrySequence ;
 #if defined HUB_TELEMETRY
 	#define USER_MAX_BYTES 6
 	uint8_t prev_index;
+
+	struct t_FrSkyD_User_Frame
+	{
+		uint8_t ID;
+		uint8_t low;
+		uint8_t high;
+	} FrSkyD_User_Frame[8];
+	uint8_t FrSkyD_User_Frame_Start=0, FrSkyD_User_Frame_End=0;
 #endif // HUB_TELEMETRY
 
 #define START_STOP	0x7e
@@ -223,6 +231,15 @@ static void multi_send_status()
 	}
 }
 
+#ifdef MULTI_CONFIG_INO
+	void CONFIG_frame()
+	{
+		multi_send_header(MULTI_TELEMETRY_CONFIG, packet_in[0]);
+		for (uint8_t i = 1; i <= packet_in[0]; i++)	// config data
+			Serial_write(packet_in[i]);
+	}
+#endif
+
 #ifdef MLINK_FW_TELEMETRY
 	void MLINK_frame()
 	{
@@ -498,6 +515,9 @@ void init_frskyd_link_telemetry()
 	TX_RSSI=0;
 	RX_LQI=0;
 	TX_LQI=0;
+	#if defined HUB_TELEMETRY
+		FrSkyD_User_Frame_Start=FrSkyD_User_Frame_End=0;
+	#endif
 }
 
 void frsky_link_frame()
@@ -575,7 +595,7 @@ packet_in[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
 0A     0F          5E 3A 06 00 5E 5E 3B 09 00 5E
 05     10          5E 06 16 72 5E 5E 3A 06 00 5E
 */
-static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t low, uint8_t high)
+static void __attribute__((unused)) frsky_write_user_frame(uint8_t ID, uint8_t low, uint8_t high)
 {
 	telemetry_in_buffer[6]  = 0x04;		// number of bytes in the payload
 	telemetry_in_buffer[7]  = 0x00;		// unknown?
@@ -596,7 +616,32 @@ static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t lo
 			telemetry_in_buffer[pos++] = value;
 		value = high;
 	}
-	telemetry_link |= 2;						// request to send frame
+	telemetry_link |= 2;				// request to send frame
+}
+
+static void __attribute__((unused)) frsky_send_user_frame(uint8_t ID, uint8_t low, uint8_t high)
+{
+	if(telemetry_link&2)
+	{ // add to buffer
+		uint8_t test = (FrSkyD_User_Frame_End + 1) & 0x07;
+		if(test == FrSkyD_User_Frame_Start)
+			return;	// buffer full...
+		FrSkyD_User_Frame_End = test;
+		FrSkyD_User_Frame[FrSkyD_User_Frame_End].ID   = ID;
+		FrSkyD_User_Frame[FrSkyD_User_Frame_End].low  = low;
+		FrSkyD_User_Frame[FrSkyD_User_Frame_End].high = high;
+	}
+	else // send to TX direct
+		frsky_write_user_frame(ID, low, high);
+}
+
+static void __attribute__((unused)) frsky_check_user_frame()
+{
+	if(telemetry_link&2 || FrSkyD_User_Frame_Start == FrSkyD_User_Frame_End)
+		return;		// need to wait that the last frame is sent or buffer is empty
+	frsky_write_user_frame(FrSkyD_User_Frame[FrSkyD_User_Frame_Start].ID, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].low, FrSkyD_User_Frame[FrSkyD_User_Frame_Start].high);
+	FrSkyD_User_Frame_Start++;
+	FrSkyD_User_Frame_Start &= 0x07;
 }
 #endif
 
@@ -905,6 +950,14 @@ void TelemetryUpdate()
 	#endif // SPORT_TELEMETRY
 
 	#ifdef MULTI_TELEMETRY
+		#if defined MULTI_CONFIG_INO
+			if(telemetry_link && protocol == PROTO_CONFIG)
+			{
+				CONFIG_frame();
+				telemetry_link=0;
+				return;
+			}
+		#endif
 		#if defined MLINK_FW_TELEMETRY
 			if(telemetry_link && protocol == PROTO_MLINK)
 			{
@@ -964,15 +1017,16 @@ void TelemetryUpdate()
 	#endif //MULTI_TELEMETRY
 	
 	if( telemetry_link & 1 )
-	{	// FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec + Bugs + BugsMini + NCC1701 + PROPEL + RLINK + OMP + MLINK
+	{	// FrSkyD + Hubsan + AFHDS2A + Bayang + Cabell + Hitec + Bugs + BugsMini + NCC1701 + PROPEL + RLINK + OMP + MLINK + DEVO
 		// FrSkyX telemetry if in PPM
 		frsky_link_frame();
 		return;
 	}
 	#if defined HUB_TELEMETRY
-		if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG || protocol == PROTO_MLINK ) )
+		if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG || protocol == PROTO_MLINK  || protocol == PROTO_DEVO ) )
-		{	// FrSkyD + Bayang + MLINK
+		{	// FrSkyD + Bayang + MLINK + DEVO
 			frsky_user_frame();
+			frsky_check_user_frame();
 			return;
 		}
 	#endif