From 0fddd9c11906f8c72fc7d34ee7f113a76016e751 Mon Sep 17 00:00:00 2001
From: pascallanger <pascal_langer@yahoo.fr>
Date: Fri, 25 Oct 2019 22:03:39 +0200
Subject: [PATCH] Fix AFHDS2A RX_LQI forward on CH

Hopefully...
---
 Multiprotocol/AFHDS2A_a7105.ino | 21 ++++++++++++++-------
 Multiprotocol/Multiprotocol.h   |  2 +-
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino
index acdfb6f..7abc6de 100644
--- a/Multiprotocol/AFHDS2A_a7105.ino
+++ b/Multiprotocol/AFHDS2A_a7105.ino
@@ -122,7 +122,8 @@ static void AFHDS2A_update_telemetry()
 						telemetry_link=1;
 						break;
 					case AFHDS2A_SENSOR_RX_ERR_RATE:
-						RX_LQI=packet[index+2];
+						if(packet[index+2]<=100)
+							RX_LQI=packet[index+2];
 						break;
 					case AFHDS2A_SENSOR_RX_RSSI:
 						RX_RSSI = -packet[index+2];
@@ -349,13 +350,19 @@ uint16_t ReadAFHDS2A()
 					if(packet[0] == 0xAA || packet[0] == 0xAC)
 					{
 						if(!memcmp(&packet[1], rx_tx_addr, 4))
-						{ // Validate TX address
+						{ // TX address validated
 							#ifdef AFHDS2A_LQI_CH
-								for(uint8_t sensor=0; sensor<7; sensor++)
-								{//read LQI value for RX output
-									uint8_t index = 9+(4*sensor);
-									if(packet[index]==AFHDS2A_SENSOR_RX_ERR_RATE)
-										RX_LQI=packet[index+2];
+								if(packet[0]==0xAA && packet[9]!=0xFD)
+								{// Normal telemetry packet
+									for(uint8_t sensor=0; sensor<7; sensor++)
+									{//read LQI value for RX output
+										uint8_t index = 9+(4*sensor);
+										if(packet[index]==AFHDS2A_SENSOR_RX_ERR_RATE && packet[index+2]<=100)
+										{
+											RX_LQI=packet[index+2];
+											break;
+										}
+									}
 								}
 							#endif
 							#if defined(AFHDS2A_FW_TELEMETRY) || defined(AFHDS2A_HUB_TELEMETRY)
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 53532ea..2c71f06 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -19,7 +19,7 @@
 #define VERSION_MAJOR		1
 #define VERSION_MINOR		3
 #define VERSION_REVISION	0
-#define VERSION_PATCH_LEVEL	22
+#define VERSION_PATCH_LEVEL	23
 
 //******************
 // Protocols