From a917227ddcf096889aa4d69a011289e8ed56fdbe Mon Sep 17 00:00:00 2001
From: Pascal Langer <planger@mathworks.com>
Date: Thu, 4 Feb 2021 12:05:57 +0100
Subject: [PATCH] Bayang: generic frsky hub function

---
 Multiprotocol/Bayang_nrf24l01.ino | 26 +++++---------------------
 Multiprotocol/Multiprotocol.h     |  2 +-
 Multiprotocol/Multiprotocol.ino   |  5 +++++
 Multiprotocol/Telemetry.ino       | 23 +++++++++++++++++++++++
 4 files changed, 34 insertions(+), 22 deletions(-)

diff --git a/Multiprotocol/Bayang_nrf24l01.ino b/Multiprotocol/Bayang_nrf24l01.ino
index 3dcf3ee..6519ab9 100644
--- a/Multiprotocol/Bayang_nrf24l01.ino
+++ b/Multiprotocol/Bayang_nrf24l01.ino
@@ -230,32 +230,16 @@ static void __attribute__((unused)) BAYANG_check_rx(void)
 			//Flags
 			//uint8_t flags = packet[3] >> 3;
 			// battery low: flags & 1
+			telemetry_link=1;
 			#if defined HUB_TELEMETRY
 				// Multiplexed P, I, D values in packet[8] and packet[9].
     	        // The two most significant bits specify which term is sent.
-        	    // Remaining 14 bits represent the value: 0 .. 16383	telemetry_in_buffer[6] = 4;
-				telemetry_in_buffer[6]  = 0x04;
-				telemetry_in_buffer[7]  = 0x00;
-				telemetry_in_buffer[8]  = 0x5E;
-				telemetry_in_buffer[9]  = 0x24+(packet[8]>>6);	//0x24 = ACCEL_X_ID, so ACCEL_X_ID=P, ACCEL_Y_ID=I, ACCEL_Z_ID=D
-				uint8_t pos=11;
-				telemetry_in_buffer[10] = packet[9];
-				if(telemetry_in_buffer[10] == 0x5D || telemetry_in_buffer[10] == 0x5E)
-				{// Byte stuffing... I'm not sure if we really care about these 2 values, may be it would be simpler and shorter to just send the value above or below.
-					telemetry_in_buffer[11] = telemetry_in_buffer[10] ^ 0x60;
-					telemetry_in_buffer[10] = 0x5D;
-					telemetry_in_buffer[6]++;
-					pos++;
-				}
-				telemetry_in_buffer[pos] = packet[8] & 0x3F;
+        	    // Remaining 14 bits represent the value: 0 .. 16383
+				frsky_send_user_frame(0x24+(packet[8]>>6), packet[9], packet[8] & 0x3F );	//0x24 = ACCEL_X_ID, so ACCEL_X_ID=P, ACCEL_Y_ID=I, ACCEL_Z_ID=D
 			#endif
 			telemetry_counter++;
-			if(telemetry_lost==0)
-				#if defined HUB_TELEMETRY
-					telemetry_link=3;
-				#else
-					telemetry_link=1;
-				#endif
+			if(telemetry_lost)
+				telemetry_link=0;	// Don't send anything yet
 		}
 	}
 	NRF24L01_SetTxRxMode(TXRX_OFF);
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 21f47d0..a949442 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -19,7 +19,7 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		3
 #define VERSION_REVISION	2
-#define VERSION_PATCH_LEVEL	26
+#define VERSION_PATCH_LEVEL	27
 
 //******************
 // Protocols
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index c104aa4..615fb69 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -234,6 +234,11 @@ uint8_t packet_in[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets
 		uint8_t	SportHead=0, SportTail=0;
 	#endif
 
+	// Functions definition when required
+	#ifdef HUB_TELEMETRY
+		static void __attribute__((unused)) frsky_send_user_frame(uint8_t, uint8_t, uint8_t);
+	#endif
+
 	//RX protocols
 	#if defined(AFHDS2A_RX_A7105_INO) || defined(FRSKY_RX_CC2500_INO) || defined(BAYANG_RX_NRF24L01_INO) || defined(DSM_RX_CYRF6936_INO)
 		bool rx_data_started;
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index a7e55ea..f39fcc4 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -594,6 +594,29 @@ 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)
+{
+	telemetry_in_buffer[6]  = 0x04;		// number of bytes in the payload
+	telemetry_in_buffer[7]  = 0x00;		// unknown?
+	telemetry_in_buffer[8]  = 0x5E;		// start of payload
+	telemetry_in_buffer[9]  = ID;		// ID must be less than 0x40
+	uint8_t pos=10;
+	uint8_t value = low;
+	for(uint i=0;i<2;i++)
+	{// Byte stuffing
+		if(value == 0x5D || value == 0x5E)
+		{// Byte stuffing
+			telemetry_in_buffer[pos+1] = value ^ 0x60;
+			telemetry_in_buffer[pos] = 0x5D;
+			telemetry_in_buffer[6]++;	// 1 more byte in the payload
+			pos += 2;
+		}
+		else
+			telemetry_in_buffer[pos++] = value;
+		value = high;
+	}
+	telemetry_link |= 2;						// request to send frame
+}
 #endif