From 85548d6e8e134246e2c542ae26518965134459f4 Mon Sep 17 00:00:00 2001
From: pascallanger <planger@FR-PLANGER.dhcp.mathworks.com>
Date: Thu, 28 Jan 2016 17:25:15 +0100
Subject: [PATCH] Add: Telemetry display for Hubsan TX RSSI

---
 Multiprotocol/Hubsan_a7105.ino  | 32 +++++++++++++++++++++-----------
 Multiprotocol/Multiprotocol.ino | 11 +++++------
 Multiprotocol/Telemetry.ino     |  7 +++----
 3 files changed, 29 insertions(+), 21 deletions(-)

diff --git a/Multiprotocol/Hubsan_a7105.ino b/Multiprotocol/Hubsan_a7105.ino
index d13bdb7..f126b34 100644
--- a/Multiprotocol/Hubsan_a7105.ino
+++ b/Multiprotocol/Hubsan_a7105.ino
@@ -181,14 +181,16 @@ static void __attribute__((unused)) hubsan_build_packet()
 }
 
 #if defined(TELEMETRY)
-/*static uint8_t hubsan_check_integrity() 
+static __attribute__((unused)) uint8_t hubsan_check_integrity() 
 {
-    int sum = 0;
+    if( (packet[0]&0xFE) != 0xE0 )
+		return 0;
+	return 1;
+	uint8_t sum = 0;
     for(uint8_t i = 0; i < 15; i++)
         sum += packet[i];
-    return packet[15] == ((256 - (sum % 256)) & 0xFF);
+	return ( packet[15] == (uint8_t)(-sum) );
 }
-*/
 #endif
 
 uint16_t ReadHubsan() 
@@ -295,7 +297,8 @@ uint16_t ReadHubsan()
 			}
 			else {
 #if defined(TELEMETRY)
-				if( rfMode == A7105_TX) {// switch to rx mode 3ms after packet sent
+				if( rfMode == A7105_TX)
+				{// switch to rx mode 3ms after packet sent
 					for( i=0; i<10; i++)
 					{
 						if( !(A7105_ReadReg(A7105_00_MODE) & 0x01)) {// wait for tx completion
@@ -306,15 +309,23 @@ uint16_t ReadHubsan()
 						}
 					}
 				}
-				if( rfMode == A7105_RX) { // check for telemetry frame
-					for( i=0; i<10; i++) {
-						if( !(A7105_ReadReg(A7105_00_MODE) & 0x01)) { // data received
+				if( rfMode == A7105_RX)
+				{ // check for telemetry frame
+					for( i=0; i<10; i++)
+					{
+						if( !(A7105_ReadReg(A7105_00_MODE) & 0x01))
+						{ // data received
 							A7105_ReadData();
-							if( !(A7105_ReadReg(A7105_00_MODE) & 0x01)){ // data received
-								v_lipo=packet[13];// hubsan lipo voltage 8bits the real value is h_lipo/10(0x2A=42-4.2V)
+							if( hubsan_check_integrity() )
+							{
+								v_lipo=packet[13];// hubsan lipo voltage 8bits the real value is h_lipo/10(0x2A=42 -> 4.2V)
 								telemetry_link=1;
 							}	
 							A7105_Strobe(A7105_RX);
+							// Read TX RSSI
+							RSSI_dBm=256-(A7105_ReadReg(A7105_1D_RSSI_THOLD)*8)/5;		// value from A7105 is between 8 for maximum signal strength to 160 or less
+							if(RSSI_dBm<0) RSSI_dBm=0;
+							else if(RSSI_dBm>255) RSSI_dBm=255;
 							break;
 						}
 					}
@@ -344,7 +355,6 @@ uint16_t initHubsan() {
 	packet_count=0;
 	id_data=ID_NORMAL;
 #if defined(TELEMETRY)
-	v_lipo=0;
 	telemetry_link=0;
 #endif
 	return 10000;
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index c3abbda..5b927bf 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -29,7 +29,6 @@
 #include "_Config.h"
 
 //Global constants/variables
-
 uint32_t MProtocol_id;//tx id,
 uint32_t MProtocol_id_master;
 uint32_t Model_fixed_id=0;
@@ -558,11 +557,11 @@ uint16_t limit_channel_100(uint8_t ch)
 #if defined(TELEMETRY)
 void Serial_write(uint8_t data)
 {
-	uint8_t t=tx_head;
-	if(++t>=TXBUFFER_SIZE)
-		t=0;
-	tx_buff[t]=data;
-	tx_head=t;
+	cli();	// disable global int
+	if(++tx_head>=TXBUFFER_SIZE)
+		tx_head=0;
+	tx_buff[tx_head]=data;
+	sei();	// enable global int
 	UCSR0B |= (1<<UDRIE0);//enable UDRE interrupt
 }
 #endif
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index 9d4ffbf..48f8f2c 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -47,17 +47,16 @@ void frsky_link_frame()
 		frame[2] = pktt[4];
 		frame[3] = (uint8_t)RSSI_dBm; 
 		frame[4] = pktt[5]*2;
-		frame[5] = frame[6] = frame[7] = frame[8] = 0;			
 	}
 	else
 		if ((cur_protocol[0]&0x1F)==MODE_HUBSAN)
 		{	
 			frame[1] = v_lipo*2; //v_lipo; common 0x2A=42/10=4.2V
 			frame[2] = frame[1];			
-			frame[3] =0X6e;
-			frame[4] =2*0x6e;				
-			frame[5] = frame[6] = frame[7] = frame[8] = 0;
+			frame[3] = 0x00;
+			frame[4] = (uint8_t)RSSI_dBm;
 		}
+	frame[5] = frame[6] = frame[7] = frame[8] = 0;			
 	frskySendStuffed();
 }