Add reporting of input lag via telemetry message

Multiprotocol/AFHDS2A_a7105.ino

@@ -291,6 +291,7 @@ uint16_t ReadAFHDS2A()
 			}                        
 			return 3850;
 		case AFHDS2A_DATA:
+		    telemetry_set_input_sync(3850);
 			AFHDS2A_build_packet(packet_type);
 			if((A7105_ReadReg(A7105_00_MODE) & 0x01))		// Check if something has been received...
 				data_rx=0;

Multiprotocol/ASSAN_nrf24l01.ino

@@ -127,6 +127,7 @@ uint16_t ASSAN_callback()
 			phase=ASSAN_DATA2;
 			return 2000;
 		case ASSAN_DATA2:
+		    telemetry_set_input_sync(12000);
 		case ASSAN_DATA3:
 			ASSAN_send_packet();
 			phase++;	// DATA 3 or 4

Multiprotocol/Bayang_nrf24l01.ino

@@ -249,8 +249,10 @@ uint16_t BAYANG_callback()
 {
 	if(IS_BIND_DONE)
 	{
-		if(packet_count==0)
+		if(packet_count==0) {
+			telemetry_set_input_sync(10000);
 			BAYANG_send_packet(0);
+		}
 		packet_count++;
 		#ifdef BAYANG_HUB_TELEMETRY
 			if (option)

Multiprotocol/CABELL_nrf224l01.ino

@@ -403,6 +403,7 @@ static void CABELL_SetPower()    // This over-ride the standard Set Power to all
 //-----------------------------------------------------------------------------------------
 uint16_t CABELL_callback()
 {
+    telemetry_set_input_sync(CABELL_PACKET_PERIOD);
 	if (IS_BIND_DONE)
 	{
 		CABELL_send_packet(0);  // packet_period is set/adjusted in CABELL_send_packet

Multiprotocol/CG023_nrf24l01.ino

@@ -145,6 +145,7 @@ uint16_t CG023_callback()
 			BIND_DONE;
 		else
 		{
+		    telemetry_set_input_sync(packet_period);
 			CG023_send_packet(1);
 			bind_counter--;
 		}

Multiprotocol/CX10_nrf24l01.ino

@@ -229,6 +229,7 @@ uint16_t CX10_callback()
 			}
 			break;
 		case CX10_DATA:
+		    telemetry_set_input_sync(packet_period);
 			CX10_Write_Packet(0);
 			break;
 	}

Multiprotocol/DM002_nrf24l01.ino

@@ -112,9 +112,10 @@ static void __attribute__((unused)) DM002_init()
 
 uint16_t DM002_callback()
 {
-	if(IS_BIND_DONE)
+    if(IS_BIND_DONE) {
+        telemetry_set_input_sync(DM002_PACKET_PERIOD);
 		DM002_send_packet(0);
-	else
+	} else
 	{
 		if (bind_counter == 0)
 		{

Multiprotocol/DSM_cyrf6936.ino

@@ -436,6 +436,7 @@ uint16_t ReadDsm()
 			DSM_set_sop_data_crc();
 			return 10000;
 		case DSM_CH1_WRITE_A:
+		    telemetry_set_input_sync(11000); //Always request 11ms spacing even if we don't use half of it in 22ms mode
 		case DSM_CH1_WRITE_B:
 		case DSM_CH2_WRITE_A:
 		case DSM_CH2_WRITE_B:

Multiprotocol/Devo_cyrf6936.ino

@@ -276,6 +276,7 @@ uint16_t devo_callback()
 	if (txState == 0)
 	{
 		txState = 1;
+	telemetry_set_input_sync(2400);
 		DEVO_BuildPacket();
 		CYRF_WriteDataPacket(packet);
 		return 1200;

Multiprotocol/ESky_nrf24l01.ino

@@ -143,6 +143,7 @@ uint16_t ESKY_callback()
 	{
 		if (packet_sent && NRF24L01_packet_ack() != PKT_ACKED)
 			return ESKY_PACKET_CHKTIME;
+        telemetry_set_input_sync(ESKY_PACKET_PERIOD);
 		ESKY_send_packet(0);
 	}
 	else

Multiprotocol/FQ777_nrf24l01.ino

@@ -187,8 +187,10 @@ uint16_t FQ777_callback()
 			BIND_DONE;
 		}
 	}
-	else
+	else {
+	    telemetry_set_input_sync(FQ777_PACKET_PERIOD);
 		FQ777_send_packet(0);
+	}
 	return FQ777_PACKET_PERIOD;
 }
 

Multiprotocol/FY326_nrf24l01.ino

@@ -188,6 +188,7 @@ uint16_t FY326_callback()
 				return FY326_PACKET_CHKTIME;
 			break;
 		case FY326_DATA:
+		    telemetry_set_input_sync(FY326_PACKET_PERIOD);
 			FY326_send_packet(0);
 			break;
 	}

Multiprotocol/FlySky_a7105.ino

@@ -158,7 +158,13 @@ uint16_t ReadFlySky()
 	#ifndef FORCE_FLYSKY_TUNING
 		A7105_AdjustLOBaseFreq(1);
 	#endif
-	if(IS_BIND_IN_PROGRESS)
+
+    uint16_t packet_period = 1510;	//1460 on deviation but not working with the latest V911 bricks... Turnigy 9X v2 is 1533, Flysky TX for 9XR/9XR Pro is 1510, V911 TX is 1490.
+
+    if(sub_protocol==CX20)
+		packet_period=3984;
+
+	if(IS_BIND_DONE)
 	{
 		flysky_build_packet(1);
 		A7105_WriteData(21, 1);
@@ -168,16 +174,14 @@ uint16_t ReadFlySky()
 	}
 	else
 	{
+	    telemetry_set_input_sync(packet_period);
 		flysky_build_packet(0);
 		A7105_WriteData(21, hopping_frequency[hopping_frequency_no & 0x0F]);
 		A7105_SetPower();
 	}
 	hopping_frequency_no++;
 
-	if(sub_protocol==CX20)
+    return packet_period;
-		return 3984;
-	else
-		return 1510;	//1460 on deviation but not working with the latest V911 bricks... Turnigy 9X v2 is 1533, Flysky TX for 9XR/9XR Pro is 1510, V911 TX is 1490.
 }
 
 const uint8_t PROGMEM tx_channels[8][4] = {

Multiprotocol/FrSkyD_cc2500.ino

@@ -156,6 +156,7 @@ uint16_t ReadFrSky_2way()
 	{
 		if (state == FRSKY_DATA1)
 		{
+		telemetry_set_input_sync(9000);
 			len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
 			if (len && len<=(0x11+3))// 20bytes
 			{		

Multiprotocol/FrSkyV_cc2500.ino

@@ -125,6 +125,7 @@ uint16_t ReadFRSKYV()
 			prev_option=option;
 		}
 		CC2500_WriteReg(CC2500_0A_CHANNR, chan * 5 + 6);
+		telemetry_set_input_sync(9006);
 		FRSKYV_build_data_packet();
 
 		if (phase == FRSKYV_DATA5)

Multiprotocol/FrSkyX_cc2500.ino

@@ -310,6 +310,7 @@ uint16_t ReadFrSkyX()
 				CC2500_Strobe(CC2500_SFRX);			//flush the RXFIFO
 			}
 			frskyX_data_frame();
+			telemetry_set_input_sync(9000);
 			if ( FrX_send_seq != 0x08 )
 			{
 				FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ;

Multiprotocol/GW008_nrf24l01.ino

@@ -139,6 +139,7 @@ uint16_t GW008_callback()
 			return 5000;
 			break;
 		case GW008_DATA:
+		    telemetry_set_input_sync(GW008_PACKET_PERIOD);
 			send_packet(0);
 			break;
 	}

Multiprotocol/H8_3D_nrf24l01.ino

@@ -176,6 +176,7 @@ static void __attribute__((unused)) H8_3D_init()
 
 uint16_t H8_3D_callback()
 {
+    telemetry_set_input_sync(packet_period);
 	if(IS_BIND_DONE)
 		H8_3D_send_packet(0);
 	else

Multiprotocol/Hisky_nrf24l01.ino

@@ -223,6 +223,10 @@ uint16_t hisky_cb()
 		default:
 			//Send normal packet
 			phase = 0;
+			if (sub_protocol==HK310)
+			    telemetry_set_input_sync(5000);
+			else
+			    telemetry_set_input_sync(9000);
 			NRF24L01_WritePayload(packet,10);
 			break;
 	}

Multiprotocol/Hontai_nrf24l01.ino

@@ -231,6 +231,7 @@ static void __attribute__((unused)) HONTAI_initialize_txid()
 
 uint16_t HONTAI_callback()
 {
+    uint16_t packet_period = (sub_protocol == FORMAT_FQ777_951) ? FQ777_951_PACKET_PERIOD : HONTAI_PACKET_PERIOD;
 	if(bind_counter!=0)
 	{
 		HONTAI_send_packet(1);
@@ -241,10 +242,12 @@ uint16_t HONTAI_callback()
 			BIND_DONE;
 		}
 	}
-	else
+	else {
+	    telemetry_set_input_sync(packet_period);
 		HONTAI_send_packet(0);
+	}
 
-	return sub_protocol == FORMAT_FQ777_951 ? FQ777_951_PACKET_PERIOD : HONTAI_PACKET_PERIOD;
+	return packet_period;
 }
 
 uint16_t initHONTAI()

Multiprotocol/Hubsan_a7105.ino

@@ -361,6 +361,7 @@ uint16_t ReadHubsan()
 #endif
 				if( phase == DATA_1)
 						A7105_SetPower(); //Keep transmit power in sync
+			    telemetry_set_input_sync(10000);
 				hubsan_build_packet();
 				A7105_Strobe(A7105_STANDBY);
 				uint8_t ch;

Multiprotocol/J6Pro_cyrf6936.ino

@@ -203,6 +203,7 @@ uint16_t ReadJ6Pro()
         case J6PRO_CHAN_1:
             //Keep transmit power updated
             CYRF_SetPower(0x28);
+            telemetry_set_input_sync(24550);
             j6pro_build_data_packet();
             //return 3400;
         case J6PRO_CHAN_2:

Multiprotocol/KN_nrf24l01.ino

@@ -321,6 +321,7 @@ uint16_t kn_callback()
 				packet_sent = 0;
 				hopping_frequency_no++;
 				if(hopping_frequency_no >= KN_RF_CH_COUNT) hopping_frequency_no = 0;
+				telemetry_set_input_sync(packet_count * packet_period);
 				kn_update_packet_control_data();
 				NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no]);
 			}

Multiprotocol/MJXQ_nrf24l01.ino

@@ -299,9 +299,10 @@ static void __attribute__((unused)) MJXQ_initialize_txid()
 
 uint16_t MJXQ_callback()
 {
-	if(IS_BIND_DONE)
+    if(IS_BIND_DONE) {
+	    telemetry_set_input_sync(MJXQ_PACKET_PERIOD);
 		MJXQ_send_packet(0);
-	else
+	} else
 	{
 		if (bind_counter == 0)
 		{

Multiprotocol/MT99xx_nrf24l01.ino

@@ -221,9 +221,10 @@ static void __attribute__((unused)) MT99XX_initialize_txid()
 
 uint16_t MT99XX_callback()
 {
-	if(IS_BIND_DONE)
+	if(IS_BIND_DONE) {
+	    telemetry_set_input_sync(packet_period);
 		MT99XX_send_packet();
-	else
+	} else
 	{
 		if (bind_counter == 0)
 		{

Multiprotocol/Multiprotocol.ino

@@ -68,6 +68,7 @@
 uint32_t MProtocol_id;//tx id,
 uint32_t MProtocol_id_master;
 uint32_t blink=0,last_signal=0;
+uint32_t last_serial_input=0;
 //
 uint16_t counter;
 uint8_t  channel;
@@ -521,11 +522,11 @@ void loop()
 				next_callback=2000;					// No PPM/serial signal check again in 2ms...
 			TX_MAIN_PAUSE_off;
 			tx_resume();
-			while(next_callback>4000)
+			while(next_callback>1000)
 			{ // start to wait here as much as we can...
-				next_callback-=2000;				// We will wait below for 2ms
+				next_callback-=500;				// We will wait below for 0.5ms
 				cli();								// Disable global int due to RW of 16 bits registers
-				OCR1A += 2000*2 ;					// set compare A for callback
+				OCR1A += 500*2 ;					// set compare A for callback
 				#ifndef STM32_BOARD	
 					TIFR1=OCF1A_bm;					// clear compare A=callback flag
 				#else
@@ -538,12 +539,12 @@ void loop()
 					break;
 				}
 				#ifndef STM32_BOARD
-					while((TIFR1 & OCF1A_bm) == 0);	// wait 2ms...
+					while((TIFR1 & OCF1A_bm) == 0);	// wait 0.5ms...
 				#else
-					while((TIMER2_BASE->SR & TIMER_SR_CC1IF)==0);//2ms wait
+					while((TIMER2_BASE->SR & TIMER_SR_CC1IF)==0);//0.5ms wait
 				#endif
 			}
-			// at this point we have a maximum of 4ms in next_callback
+			// at this point we have a maximum of 1ms in next_callback
 			next_callback *= 2 ;
 			cli();									// Disable global int due to RW of 16 bits registers
 			OCR1A+= next_callback ;					// set compare A for callback
@@ -659,7 +660,7 @@ static void update_channels_aux(void)
 static void update_led_status(void)
 {
 	if(IS_INPUT_SIGNAL_on)
-		if(millis()-last_signal>70)
+		if(millis()-last_signal> 70)
 			INPUT_SIGNAL_off;							//no valid signal (PPM or Serial) received for 70ms
 	if(blink<millis())
 	{
@@ -1256,6 +1257,7 @@ void read_multimodule_config() {
 
 void parse_serial_multi_command()
 {
+	debug("multi command type %d, len %d", rx_buff[2], rx_buff[3]);
 	// Header 'M', 'P, Type, Len
 	if (rx_buff[1] != 'P')
 		return;
@@ -1274,6 +1276,7 @@ void parse_serial_multi_command()
             Serial_write (newconfig);
 			if (newconfig != multi_config)
 			{
+				debug("New multimodule config: %x -> %x", multi_config, newconfig);
 				multi_config = newconfig;
 				eeprom_write_byte(CONFIG_EEPROM_OFFSET, newconfig);
 				// Reinit serial port to enable/disable inversion
@@ -1282,8 +1285,8 @@ void parse_serial_multi_command()
 		}
 		break;
 #endif
-		default:
+    default:
-			;
+        ;
 	}
 }
 #endif
@@ -1346,6 +1349,13 @@ void modules_reset()
 			}
 		#endif // CHECK_FOR_BOOTLOADER
 	#elif defined STM32_BOARD
+        if (IS_TELEMTRY_INVERSION_ON) {
+            TX_INV_on;//activated inverter for both serial TX and RX signals
+            RX_INV_on;
+        } else {
+            TX_INV_off;
+            RX_INV_off;
+        }
 		#ifdef CHECK_FOR_BOOTLOADER
 			if ( boot )
 			{
@@ -1529,7 +1539,7 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
 	#ifndef FORCE_GLOBAL_ID
 		uint32_t id=0;
 
-		if(eeprom_read_byte((EE_ADDR)(address+10))==0xf0 && !create_new)
+		if(eeprom_read_byte((EE_ADDR)(address+EEPROM_ID_VALID_OFFSET))==0xf0 && !create_new)
 		{  // TXID exists in EEPROM
 			for(uint8_t i=4;i>0;i--)
 			{
@@ -1662,8 +1672,11 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
 			else
 			{
 				rx_buff[idx++]=UDR0;		// Store received byte
-				if(idx>=RXBUFFER_SIZE || (rx_buff[0] == 'M' && idx >= 4 && rx_buff[3] == idx-4)) // A full frame has been received or a long enough multicommand message
+
+				if(idx>=RXBUFFER_SIZE || (rx_buff[0] == 'M' && idx >= 4 && rx_buff[3] == idx-4))
 				{
+                    // A full frame has been received or a long enough multicommand message
+					last_serial_input=TCNT1;
 					if(!IS_RX_DONOTUPDTAE_on)
 					{ //Good frame received and main is not working on the buffer
 						memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer

Multiprotocol/Q303_nrf24l01.ino

@@ -354,9 +354,10 @@ static void __attribute__((unused)) Q303_initialize_txid()
 
 uint16_t Q303_callback()
 {
-	if(IS_BIND_DONE)
+	if(IS_BIND_DONE) {
+	    telemetry_set_input_sync(packet_period);
 		Q303_send_packet(0);
-	else
+	} else
 	{
 		if (bind_counter == 0)
 		{

Multiprotocol/SFHSS_cc2500.ino

@@ -235,6 +235,7 @@ uint16_t ReadSFHSS()
 #define SFHSS_PACKET_PERIOD	6800
 #define SFHSS_DATA2_TIMING	1625	// Adjust this value between 1600 and 1650 if your RX(s) are not operating properly
 		case SFHSS_DATA1:
+		    telemetry_set_input_sync(6800);
 			SFHSS_build_data_packet();
 			SFHSS_send_packet();
 			phase = SFHSS_DATA2;

Multiprotocol/SHENQI_nrf24l01.ino

@@ -63,6 +63,8 @@ void SHENQI_send_packet()
 	}
 	else
 	{
+	    if (packet_count==1)
+	        telemetry_set_input_sync(3000+2508+6*1750);
 		LT8900_SetAddress(rx_tx_addr,4);
 		packet[1]=255-convert_channel_8b(RUDDER);
 		packet[2]=255-convert_channel_16b_limit(THROTTLE,0x60,0xA0);

Multiprotocol/SLT_nrf24l01.ino

@@ -145,6 +145,7 @@ uint16_t SLT_callback()
 	switch (phase)
 	{
 		case SLT_BUILD:
+		    telemetry_set_input_sync(22000);
 			SLT_build_packet();
 			phase++;
 			return 1000;

Multiprotocol/Symax_nrf24l01.ino

@@ -359,6 +359,7 @@ uint16_t symax_callback()
 			}
 			break;
 		case SYMAX_DATA:
+		    telemetry_set_input_sync(SYMAX_PACKET_PERIOD);
 			SYMAX_send_packet(0);
 			break;
 	}

Multiprotocol/Telemetry.ino

@@ -24,8 +24,12 @@ uint8_t RetrySequence ;
 	#define INPUT_SYNC_TIME			100	//in ms
 	#define INPUT_ADDITIONAL_DELAY	100	// in 10µs, 100 => 1000 µs
 	uint32_t lastMulti = 0;
+	uint32_t lastInputSync = 0;
+	uint16_t inputDelay = 0;
+	uint16_t inputRefreshRate = 9000;
 #endif // MULTI_TELEMETRY/MULTI_STATUS
 
+
 #if defined SPORT_TELEMETRY	
 	#define SPORT_TIME 12000	//12ms
 	#define FRSKY_SPORT_PACKET_SIZE   8
@@ -86,6 +90,38 @@ static void multi_send_header(uint8_t type, uint8_t len)
 	Serial_write(len);
 }
 
+inline void telemetry_set_input_sync(uint16_t refreshRate)
+{
+    static int c=0;
+#if defined(STM32_BOARD)
+    if (c++%2==0)
+        SPI_CSN_on;
+    else
+       SPI_CSN_off;
+#endif
+    // Only record input Delay after a frame has really been received
+    // Otherwise protocols with faster refresh rates then the TX sends (e.g. 3ms vs 6ms) will screw up the calcualtion
+    inputRefreshRate = refreshRate;
+    if (last_serial_input != 0) {
+        inputDelay = (TCNT1 - last_serial_input)/2;
+        if(inputDelay > 0x8000)
+            inputDelay =inputDelay - 0x8000;
+        last_serial_input=0;
+    }
+
+}
+
+static void mult_send_inputsync()
+{
+    multi_send_header(MULTI_TELEMETRY_INPUTSYNC, 6);
+    Serial_write(inputRefreshRate >> 8);
+    Serial_write(inputRefreshRate & 0xff);
+    Serial_write(inputDelay >> 8);
+    Serial_write(inputDelay & 0xff);
+    Serial_write(INPUT_SYNC_TIME);
+    Serial_write(INPUT_ADDITIONAL_DELAY);
+}
+
 static void multi_send_status()
 {
 	#ifdef SPORT_POLLING
@@ -847,6 +883,10 @@ void TelemetryUpdate()
 				multi_send_status();
 				lastMulti = now;
 				return;
+			} else if (IS_EXTRA_TELEMETRY_ON && now - lastInputSync > INPUT_SYNC_TIME) {
+			    mult_send_inputsync();
+			    lastInputSync = now;
+			    return;
 			}
 		}
 	#endif

Multiprotocol/V2X2_nrf24l01.ino

@@ -264,6 +264,7 @@ uint16_t ReadV2x2()
 		case V202_DATA:
 			if (packet_sent && NRF24L01_packet_ack() != PKT_ACKED)
 				return V2X2_PACKET_CHKTIME;
+			telemetry_set_input_sync(V2X2_PACKET_PERIOD);
 			V2X2_send_packet(0);
 			break;
 	}

Multiprotocol/WK2x01_cyrf6936.ino

@@ -436,6 +436,7 @@ uint16_t WK_cb()
 	if (packet_sent == 0)
 	{
 		packet_sent = 1;
+		telemetry_set_input_sync(3800);
 		if(sub_protocol == WK2801)
 			WK_BuildPacket_2801();
 		else if(sub_protocol == WK2401)

Multiprotocol/YD717_nrf24l01.ino

@@ -154,9 +154,10 @@ static void __attribute__((unused)) yd717_init()
 
 uint16_t yd717_callback()
 {
-	if(IS_BIND_DONE)
+	if(IS_BIND_DONE) {
+	    telemetry_set_input_sync(YD717_PACKET_PERIOD);
 		yd717_send_packet(0);
-	else
+	} else
 	{
 		if (bind_counter == 0)
 		{