diff --git a/Multiprotocol/A7105_SPI.ino b/Multiprotocol/A7105_SPI.ino
index f6ac130..56d31f8 100644
--- a/Multiprotocol/A7105_SPI.ino
+++ b/Multiprotocol/A7105_SPI.ino
@@ -29,8 +29,8 @@ void A7105_WriteData(uint8_t len, uint8_t channel)
 	A7105_CSN_on;
 	if(protocol!=MODE_FLYSKY)
 	{
-		A7105_Strobe(A7105_STANDBY);
-		A7105_SetTxRxMode(TX_EN);
+		A7105_Strobe(A7105_STANDBY);	//Force standby mode, ie cancel any TX or RX...
+		A7105_SetTxRxMode(TX_EN);		//Switch to PA
 	}
 	A7105_WriteReg(A7105_0F_PLL_I, channel);
 	A7105_Strobe(A7105_TX);
diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino
index c83a067..df2f1af 100644
--- a/Multiprotocol/AFHDS2A_a7105.ino
+++ b/Multiprotocol/AFHDS2A_a7105.ino
@@ -204,16 +204,12 @@ static void AFHDS2A_build_packet(uint8_t type)
 }
 
 #define AFHDS2A_WAIT_WRITE 0x80
-#ifdef STM32_BOARD
-	#define AFHDS2A_TIMING_OFFSET 0
-#else
-	#define AFHDS2A_TIMING_OFFSET 100
-#endif
 uint16_t ReadAFHDS2A()
 {
 	static uint8_t packet_type = AFHDS2A_PACKET_STICKS;
 	static uint16_t packet_counter=0;
-	
+	uint8_t data_rx;
+	uint16_t start;
 	switch(phase)
 	{
 		case AFHDS2A_BIND1:
@@ -240,17 +236,22 @@ uint16_t ReadAFHDS2A()
 			}
 			packet_count++;
 			phase |= AFHDS2A_WAIT_WRITE;
-			return 1700+AFHDS2A_TIMING_OFFSET;
+			return 1700;
 		case AFHDS2A_BIND1|AFHDS2A_WAIT_WRITE:
 		case AFHDS2A_BIND2|AFHDS2A_WAIT_WRITE:
 		case AFHDS2A_BIND3|AFHDS2A_WAIT_WRITE:
+			//Wait for TX completion
+			start=micros();
+			while ((uint16_t)micros()-start < 700)			// Wait max 700µs, using serial+telemetry exit in about 120µs
+				if(!(A7105_ReadReg(A7105_00_MODE) & 0x01))
+					break;
 			A7105_SetTxRxMode(RX_EN);
 			A7105_Strobe(A7105_RX);
 			phase &= ~AFHDS2A_WAIT_WRITE;
 			phase++;
 			if(phase > AFHDS2A_BIND3)
 				phase = AFHDS2A_BIND1;
-			return 2150-AFHDS2A_TIMING_OFFSET;
+			return 2150;
 		case AFHDS2A_BIND4:
 			AFHDS2A_build_bind_packet();
 			A7105_WriteData(AFHDS2A_TXPACKET_SIZE, packet_count%2 ? 0x0d : 0x8c);
@@ -267,6 +268,10 @@ uint16_t ReadAFHDS2A()
 			return 3850;
 		case AFHDS2A_DATA:    
 			AFHDS2A_build_packet(packet_type);
+			if((A7105_ReadReg(A7105_00_MODE) & 0x01))		// Check if something has been received...
+				data_rx=0;
+			else
+				data_rx=1;									// Yes
 			A7105_WriteData(AFHDS2A_TXPACKET_SIZE, hopping_frequency[hopping_frequency_no++]);
 			if(hopping_frequency_no >= AFHDS2A_NUMFREQ)
 				hopping_frequency_no = 0;
@@ -275,30 +280,40 @@ uint16_t ReadAFHDS2A()
 			else if(!(packet_counter % 1569))
 				packet_type = AFHDS2A_PACKET_FAILSAFE;
 			else
-				packet_type = AFHDS2A_PACKET_STICKS; // todo : check for settings changes
-			if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)))
-			{ // FECF+CRCF Ok
-				A7105_ReadData(1);
+				packet_type = AFHDS2A_PACKET_STICKS;		// todo : check for settings changes
+			if(!(A7105_ReadReg(A7105_00_MODE) & (1<<5 | 1<<6)) && data_rx==1)
+			{ // RX+FECF+CRCF Ok
+				A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
 				if(packet[0] == 0xaa)
 				{
-					A7105_Strobe(A7105_RST_RDPTR);
-					A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
 					if(packet[9] == 0xfc)
 						packet_type=AFHDS2A_PACKET_SETTINGS;	// RX is asking for settings
 					#if defined(TELEMETRY)
 					else
+					{
+						// Read TX RSSI
+						int16_t temp=256-(A7105_ReadReg(A7105_1D_RSSI_THOLD)*8)/5;		// value from A7105 is between 8 for maximum signal strength to 160 or less
+						if(temp<0) temp=0;
+						else if(temp>255) temp=255;
+						TX_RSSI=temp;
 						AFHDS2A_update_telemetry();
+					}
 					#endif
 				}
 			}
 			packet_counter++;
 			phase |= AFHDS2A_WAIT_WRITE;
-			return 1700+AFHDS2A_TIMING_OFFSET;
+			return 1700;
 		case AFHDS2A_DATA|AFHDS2A_WAIT_WRITE:
+			//Wait for TX completion
+			start=micros();
+			while ((uint16_t)micros()-start < 700)			// Wait max 700µs, using serial+telemetry exit in about 120µs
+				if(!(A7105_ReadReg(A7105_00_MODE) & 0x01))
+					break;
 			A7105_SetTxRxMode(RX_EN);
 			A7105_Strobe(A7105_RX);
 			phase &= ~AFHDS2A_WAIT_WRITE;
-			return 2150-AFHDS2A_TIMING_OFFSET;
+			return 2150;
 	}
 	return 3850; // never reached, please the compiler
 }
diff --git a/Multiprotocol/Hubsan_a7105.ino b/Multiprotocol/Hubsan_a7105.ino
index 5b98e5d..249f220 100644
--- a/Multiprotocol/Hubsan_a7105.ino
+++ b/Multiprotocol/Hubsan_a7105.ino
@@ -320,9 +320,10 @@ uint16_t ReadHubsan()
 							}	
 							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;
+							int16_t temp=256-(A7105_ReadReg(A7105_1D_RSSI_THOLD)*8)/5;		// value from A7105 is between 8 for maximum signal strength to 160 or less
+							if(temp<0) temp=0;
+							else if(temp>255) temp=255;
+							TX_RSSI=temp;
 							break;
 						}
 					}
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index b6ac0ac..ba8f07a 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -147,6 +147,7 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets
 	#endif // BASH_SERIAL
 	uint8_t v_lipo;
 	int16_t RSSI_dBm;
+	uint8_t TX_RSSI;
 	uint8_t telemetry_link=0; 
 	uint8_t telemetry_counter=0;
 	uint8_t telemetry_lost;
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index c25da8a..a09ed50 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -135,7 +135,7 @@ void frsky_link_frame()
 			frame[1] = v_lipo*2; //v_lipo; common 0x2A=42/10=4.2V
 			frame[2] = frame[1];			
 			frame[3] = protocol==MODE_HUBSAN?0x00:(uint8_t)RSSI_dBm;
-			frame[4] = protocol==MODE_HUBSAN?(uint8_t)RSSI_dBm:0x00;
+			frame[4] = TX_RSSI;
 		}
 	frame[5] = frame[6] = frame[7] = frame[8] = 0;			
 	frskySendStuffed();