From c866d077439bc653182a805c5d5ba33564a6ac2f Mon Sep 17 00:00:00 2001
From: Pascal Langer <planger@mathworks.com>
Date: Wed, 3 Feb 2021 19:44:59 +0100
Subject: [PATCH] Bayang: add PID to telemetry

---
 Multiprotocol/Bayang_nrf24l01.ino | 29 ++++++++++++++++++++++++++---
 Multiprotocol/Multiprotocol.h     |  2 +-
 Multiprotocol/Telemetry.ino       |  8 ++++----
 Protocols_Details.md              |  2 +-
 4 files changed, 32 insertions(+), 9 deletions(-)

diff --git a/Multiprotocol/Bayang_nrf24l01.ino b/Multiprotocol/Bayang_nrf24l01.ino
index 2b02cca..3dcf3ee 100644
--- a/Multiprotocol/Bayang_nrf24l01.ino
+++ b/Multiprotocol/Bayang_nrf24l01.ino
@@ -218,7 +218,7 @@ static void __attribute__((unused)) BAYANG_check_rx(void)
 		for (uint8_t i=1; i < BAYANG_PACKET_SIZE-1; i++)
 			check += packet[i];
 		// decode data , check sum is ok as well, since there is no crc
-		if (packet[0] == 0x85 && packet[14] == check)
+		if (packet[0] == 0x85 && packet[14] == check && telemetry_link == 0)
 		{
 			// uncompensated battery volts*100/2
 			v_lipo1 = (packet[3]<<7) + (packet[4]>>1);
@@ -230,9 +230,32 @@ static void __attribute__((unused)) BAYANG_check_rx(void)
 			//Flags
 			//uint8_t flags = packet[3] >> 3;
 			// battery low: flags & 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;
+			#endif
 			telemetry_counter++;
 			if(telemetry_lost==0)
-				telemetry_link=1;
+				#if defined HUB_TELEMETRY
+					telemetry_link=3;
+				#else
+					telemetry_link=1;
+				#endif
 		}
 	}
 	NRF24L01_SetTxRxMode(TXRX_OFF);
@@ -367,4 +390,4 @@ uint16_t initBAYANG(void)
 	return BAYANG_INITIAL_WAIT+BAYANG_PACKET_PERIOD;
 }
 
-#endif
+#endif
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index ea117ea..21f47d0 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	25
+#define VERSION_PATCH_LEVEL	26
 
 //******************
 // Protocols
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 43387bf..a7e55ea 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -535,7 +535,7 @@ void frsky_link_frame()
 		frame[1] = v_lipo1;
 		frame[2] = v_lipo2;
 		frame[3] = RX_RSSI;
-		telemetry_link=0;
+		telemetry_link &= ~1 ;		// Sent
 	}
 	frame[4] = TX_RSSI;
 	frame[5] = RX_LQI;
@@ -563,7 +563,7 @@ void frsky_user_frame()
 		else
 		{
 			frame[1]=telemetry_in_buffer[6];			// packet size
-			telemetry_link=0;			// only 1 packet or processing second packet
+			telemetry_link &= ~2;			// only 1 packet or processing second packet
 		}
 		frame[2] = telemetry_in_buffer[7];
 		for(uint8_t i=0;i<USER_MAX_BYTES;i++)
@@ -578,7 +578,7 @@ void frsky_user_frame()
 		#endif
 	}
 	else
-		telemetry_link=0;
+		telemetry_link &= ~2;
 }
 /*
 HuB RX packets.
@@ -958,7 +958,7 @@ void TelemetryUpdate()
 			return;
 		}
 	#if defined HUB_TELEMETRY
-		if((telemetry_link & 2) && protocol == PROTO_FRSKYD)
+		if((telemetry_link & 2) && ( protocol == PROTO_FRSKYD || protocol == PROTO_BAYANG ) )
 		{	// FrSkyD
 			frsky_user_frame();
 			return;
diff --git a/Protocols_Details.md b/Protocols_Details.md
index 54c7fbc..0e9f55b 100644
--- a/Protocols_Details.md
+++ b/Protocols_Details.md
@@ -910,7 +910,7 @@ Models: Eachine H8(C) mini, BayangToys X6/X7/X9, JJRC JJ850, Floureon H101 ...
 
 Option=0 -> normal Bayang protocol
 
-Option=1 -> enable telemetry with [Silverxxx firmware](https://github.com/silver13/H101-acro/tree/master). Value returned to the TX using FrSkyD Hub are RX RSSI, TX RSSI, A1=uncompensated battery voltage (set the ratio to 5.0 and adjust with offset), A2=compensated battery voltage (set the ratio to 5.0 and adjust with offset)
+Option=1 -> enable telemetry with [Silverxxx firmware](https://github.com/silver13/H101-acro/tree/master). Value returned to the TX using FrSkyD Hub are RX RSSI, TX RSSI, A1=uncompensated battery voltage (set the ratio to 5.0 and adjust with offset), A2=compensated battery voltage (set the ratio to 5.0 and adjust with offset) and if supported AccX=P, AccY=I, ACCZ=D
 
 Option=2 -> enable analog aux channels with [NFE Silverware firmware](https://github.com/NotFastEnuf/NFE_Silverware). Two otherwise static bytes in the protocol overridden to add two 'analog' (non-binary) auxiliary channels.