From 99e8be227ef5a374ceaf897b192069ac64859a6e Mon Sep 17 00:00:00 2001
From: pascallanger <planger@mathworks.com>
Date: Fri, 16 Sep 2016 10:39:44 +0200
Subject: [PATCH] PPM Telemetry: added serial speeds

Only supported for none invert telemetry:
FrSkyD (Incl Hubsan): 9600bps
FrSkyX: 57600bps
DSM: 125000bps
---
 Multiprotocol/Multiprotocol.h   |  2 +
 Multiprotocol/Multiprotocol.ino |  7 ++-
 Multiprotocol/Telemetry.ino     | 75 ++++++++++++++++++++++++---------
 3 files changed, 63 insertions(+), 21 deletions(-)

diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 4668f65..195797d 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -514,6 +514,8 @@ enum {
 // baudrate defines for serial
 #define SPEED_100K	0
 #define SPEED_9600	1
+#define SPEED_57600	2
+#define SPEED_125K	3
 
 //****************************************
 //*** MULTI protocol serial definition ***
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index 51cc5b3..ac9d8ad 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -855,7 +855,12 @@ void Mprotocol_serial_init()
 #if defined(TELEMETRY)
 void PPM_Telemetry_serial_init()
 {
-	initTXSerial( SPEED_9600 ) ;
+	if( (protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN))
+		initTXSerial( SPEED_9600 ) ;
+	if(protocol==MODE_FRSKYX)
+		initTXSerial( SPEED_57600 ) ;
+	if(protocol==MODE_DSM)
+		initTXSerial( SPEED_125K ) ;
 }
 #endif
 
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 6476339..44ea947 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -504,28 +504,63 @@ void Serial_write(uint8_t data)
 	tx_resume();
 }
 
-// Speed is 0 for 100K and 1 for 9600
 void initTXSerial( uint8_t speed)
 {
-	if(speed==SPEED_9600)
-	{ // 9600
-	#ifdef XMEGA
-		USARTC0.BAUDCTRLA = 207 ;
-		USARTC0.BAUDCTRLB = 0 ;
-		
-		USARTC0.CTRLB = 0x18 ;
-		USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
-		USARTC0.CTRLC = 0x03 ;
-	}
-	#else
-		//9600 bauds
-		UBRR0H = 0x00;
-		UBRR0L = 0x67;
-		UCSR0A = 0 ;	// Clear X2 bit
-		//Set frame format to 8 data bits, none, 1 stop bit
-		UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
-	}
-	UCSR0B |= (1<<TXEN0);//tx enable
+	#ifdef ENABLE_PPM
+		if(speed==SPEED_9600)
+		{ // 9600
+			#ifdef XMEGA
+				USARTC0.BAUDCTRLA = 207 ;
+				USARTC0.BAUDCTRLB = 0 ;
+				USARTC0.CTRLB = 0x18 ;
+				USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
+				USARTC0.CTRLC = 0x03 ;
+			#else
+				//9600 bauds
+				UBRR0H = 0x00;
+				UBRR0L = 0x67;
+				UCSR0A = 0 ;	// Clear X2 bit
+				//Set frame format to 8 data bits, none, 1 stop bit
+				UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
+			#endif
+		}
+		else if(speed==SPEED_57600)
+		{ // 57600
+			#ifdef XMEGA
+				/*USARTC0.BAUDCTRLA = 207 ;
+				USARTC0.BAUDCTRLB = 0 ;
+				USARTC0.CTRLB = 0x18 ;
+				USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
+				USARTC0.CTRLC = 0x03 ;*/
+			#else
+				//57600 bauds
+				UBRR0H = 0x00;
+				UBRR0L = 0x22;
+				UCSR0A = 0x02 ;	// Set X2 bit
+				//Set frame format to 8 data bits, none, 1 stop bit
+				UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
+			#endif
+		}
+		else if(speed==SPEED_125K)
+		{ // 125000
+			#ifdef XMEGA
+				/*USARTC0.BAUDCTRLA = 207 ;
+				USARTC0.BAUDCTRLB = 0 ;
+				USARTC0.CTRLB = 0x18 ;
+				USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
+				USARTC0.CTRLC = 0x03 ;*/
+			#else
+				//125000 bauds
+				UBRR0H = 0x00;
+				UBRR0L = 0x07;
+				UCSR0A = 0x00 ;	// Clear X2 bit
+				//Set frame format to 8 data bits, none, 1 stop bit
+				UCSR0C = (1<<UCSZ01)|(1<<UCSZ00);
+			#endif
+		}
+	#endif
+	#ifndef XMEGA
+		UCSR0B |= (1<<TXEN0);//tx enable
 	#endif
 }