From f5d2ce87f2e6ec36b998596c053b65214cd8d44a Mon Sep 17 00:00:00 2001 From: Arne Schwabe Date: Thu, 4 May 2017 05:38:09 +0200 Subject: [PATCH] Add reporting of input lag via telemetry message --- Multiprotocol/AFHDS2A_a7105.ino | 3 ++- Multiprotocol/ASSAN_nrf24l01.ino | 1 + Multiprotocol/Bayang_nrf24l01.ino | 4 ++- Multiprotocol/CABELL_nrf224l01.ino | 3 ++- Multiprotocol/CG023_nrf24l01.ino | 1 + Multiprotocol/CX10_nrf24l01.ino | 1 + Multiprotocol/DM002_nrf24l01.ino | 5 ++-- Multiprotocol/DSM_cyrf6936.ino | 1 + Multiprotocol/Devo_cyrf6936.ino | 1 + Multiprotocol/ESky_nrf24l01.ino | 1 + Multiprotocol/FQ777_nrf24l01.ino | 4 ++- Multiprotocol/FY326_nrf24l01.ino | 1 + Multiprotocol/FlySky_a7105.ino | 14 +++++++---- Multiprotocol/FrSkyD_cc2500.ino | 1 + Multiprotocol/FrSkyV_cc2500.ino | 1 + Multiprotocol/FrSkyX_cc2500.ino | 3 ++- Multiprotocol/GW008_nrf24l01.ino | 1 + Multiprotocol/H8_3D_nrf24l01.ino | 1 + Multiprotocol/Hisky_nrf24l01.ino | 4 +++ Multiprotocol/Hontai_nrf24l01.ino | 7 ++++-- Multiprotocol/Hubsan_a7105.ino | 1 + Multiprotocol/J6Pro_cyrf6936.ino | 1 + Multiprotocol/KN_nrf24l01.ino | 1 + Multiprotocol/MJXQ_nrf24l01.ino | 5 ++-- Multiprotocol/MT99xx_nrf24l01.ino | 5 ++-- Multiprotocol/Multiprotocol.ino | 39 +++++++++++++++++++---------- Multiprotocol/Pins.h | 2 +- Multiprotocol/Q303_nrf24l01.ino | 5 ++-- Multiprotocol/SFHSS_cc2500.ino | 1 + Multiprotocol/SHENQI_nrf24l01.ino | 2 ++ Multiprotocol/SLT_nrf24l01.ino | 1 + Multiprotocol/Symax_nrf24l01.ino | 1 + Multiprotocol/Telemetry.ino | 40 ++++++++++++++++++++++++++++++ Multiprotocol/V2X2_nrf24l01.ino | 1 + Multiprotocol/WK2x01_cyrf6936.ino | 1 + Multiprotocol/YD717_nrf24l01.ino | 7 +++--- 36 files changed, 134 insertions(+), 37 deletions(-) diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino index f9d8dd0..6dc9cab 100644 --- a/Multiprotocol/AFHDS2A_a7105.ino +++ b/Multiprotocol/AFHDS2A_a7105.ino @@ -290,7 +290,8 @@ uint16_t ReadAFHDS2A() BIND_DONE; } return 3850; - case AFHDS2A_DATA: + 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; diff --git a/Multiprotocol/ASSAN_nrf24l01.ino b/Multiprotocol/ASSAN_nrf24l01.ino index 4504654..abbacd5 100644 --- a/Multiprotocol/ASSAN_nrf24l01.ino +++ b/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 diff --git a/Multiprotocol/Bayang_nrf24l01.ino b/Multiprotocol/Bayang_nrf24l01.ino index e294da5..03114ef 100644 --- a/Multiprotocol/Bayang_nrf24l01.ino +++ b/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) diff --git a/Multiprotocol/CABELL_nrf224l01.ino b/Multiprotocol/CABELL_nrf224l01.ino index ce48808..35e30a4 100644 --- a/Multiprotocol/CABELL_nrf224l01.ino +++ b/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 @@ -439,4 +440,4 @@ uint16_t initCABELL(void) return packet_period; } -#endif \ No newline at end of file +#endif diff --git a/Multiprotocol/CG023_nrf24l01.ino b/Multiprotocol/CG023_nrf24l01.ino index 77f4509..82461f2 100644 --- a/Multiprotocol/CG023_nrf24l01.ino +++ b/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--; } diff --git a/Multiprotocol/CX10_nrf24l01.ino b/Multiprotocol/CX10_nrf24l01.ino index 8a35cba..911dacd 100644 --- a/Multiprotocol/CX10_nrf24l01.ino +++ b/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; } diff --git a/Multiprotocol/DM002_nrf24l01.ino b/Multiprotocol/DM002_nrf24l01.ino index c87d89d..c45cdeb 100644 --- a/Multiprotocol/DM002_nrf24l01.ino +++ b/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) { diff --git a/Multiprotocol/DSM_cyrf6936.ino b/Multiprotocol/DSM_cyrf6936.ino index 98fc67f..976c072 100644 --- a/Multiprotocol/DSM_cyrf6936.ino +++ b/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: diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino index 467e123..cb47e22 100644 --- a/Multiprotocol/Devo_cyrf6936.ino +++ 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; diff --git a/Multiprotocol/ESky_nrf24l01.ino b/Multiprotocol/ESky_nrf24l01.ino index 9a72c94..2225ec4 100644 --- a/Multiprotocol/ESky_nrf24l01.ino +++ b/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 diff --git a/Multiprotocol/FQ777_nrf24l01.ino b/Multiprotocol/FQ777_nrf24l01.ino index a40c4e5..23ea2c1 100644 --- a/Multiprotocol/FQ777_nrf24l01.ino +++ b/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; } diff --git a/Multiprotocol/FY326_nrf24l01.ino b/Multiprotocol/FY326_nrf24l01.ino index 117c7fe..40f6ead 100644 --- a/Multiprotocol/FY326_nrf24l01.ino +++ b/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; } diff --git a/Multiprotocol/FlySky_a7105.ino b/Multiprotocol/FlySky_a7105.ino index 3aa6269..73a596e 100644 --- a/Multiprotocol/FlySky_a7105.ino +++ b/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 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. + return packet_period; } const uint8_t PROGMEM tx_channels[8][4] = { diff --git a/Multiprotocol/FrSkyD_cc2500.ino b/Multiprotocol/FrSkyD_cc2500.ino index 9cfabf0..aa89e6f 100644 --- a/Multiprotocol/FrSkyD_cc2500.ino +++ b/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 { diff --git a/Multiprotocol/FrSkyV_cc2500.ino b/Multiprotocol/FrSkyV_cc2500.ino index f277b2c..bf75054 100644 --- a/Multiprotocol/FrSkyV_cc2500.ino +++ b/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) diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino index 4a317af..4e7000d 100644 --- a/Multiprotocol/FrSkyX_cc2500.ino +++ b/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 ; @@ -350,4 +351,4 @@ uint16_t initFrSkyX() FrX_receive_seq = 0 ; return 10000; } -#endif \ No newline at end of file +#endif diff --git a/Multiprotocol/GW008_nrf24l01.ino b/Multiprotocol/GW008_nrf24l01.ino index 2296730..8eb8291 100644 --- a/Multiprotocol/GW008_nrf24l01.ino +++ b/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; } diff --git a/Multiprotocol/H8_3D_nrf24l01.ino b/Multiprotocol/H8_3D_nrf24l01.ino index 8fe92f5..fdb804d 100644 --- a/Multiprotocol/H8_3D_nrf24l01.ino +++ b/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 diff --git a/Multiprotocol/Hisky_nrf24l01.ino b/Multiprotocol/Hisky_nrf24l01.ino index 2596c9a..5c71d31 100644 --- a/Multiprotocol/Hisky_nrf24l01.ino +++ b/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; } diff --git a/Multiprotocol/Hontai_nrf24l01.ino b/Multiprotocol/Hontai_nrf24l01.ino index 4e5b5ba..622fc9b 100644 --- a/Multiprotocol/Hontai_nrf24l01.ino +++ b/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() diff --git a/Multiprotocol/Hubsan_a7105.ino b/Multiprotocol/Hubsan_a7105.ino index 5f94855..622c553 100644 --- a/Multiprotocol/Hubsan_a7105.ino +++ b/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; diff --git a/Multiprotocol/J6Pro_cyrf6936.ino b/Multiprotocol/J6Pro_cyrf6936.ino index ff8b43f..f4b84c2 100644 --- a/Multiprotocol/J6Pro_cyrf6936.ino +++ b/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: diff --git a/Multiprotocol/KN_nrf24l01.ino b/Multiprotocol/KN_nrf24l01.ino index 40c8f92..34ef2bf 100644 --- a/Multiprotocol/KN_nrf24l01.ino +++ b/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]); } diff --git a/Multiprotocol/MJXQ_nrf24l01.ino b/Multiprotocol/MJXQ_nrf24l01.ino index a5f1856..916a7df 100644 --- a/Multiprotocol/MJXQ_nrf24l01.ino +++ b/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) { diff --git a/Multiprotocol/MT99xx_nrf24l01.ino b/Multiprotocol/MT99xx_nrf24l01.ino index 1d7e1b3..0b68680 100644 --- a/Multiprotocol/MT99xx_nrf24l01.ino +++ b/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) { diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index fc7bb2d..d09449c 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/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 @@ -537,13 +538,13 @@ void loop() next_callback=0; // Launch new protocol ASAP break; } - #ifndef STM32_BOARD - while((TIFR1 & OCF1A_bm) == 0); // wait 2ms... + #ifndef STM32_BOARD + 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(blink2) bind_counter=2; } - + //store current protocol values for(uint8_t i=0;i<3;i++) cur_protocol[i] = rx_ok_buff[i]; @@ -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 diff --git a/Multiprotocol/Pins.h b/Multiprotocol/Pins.h index 771cfb6..6a7dc6d 100644 --- a/Multiprotocol/Pins.h +++ b/Multiprotocol/Pins.h @@ -346,4 +346,4 @@ #define eeprom_read_byte EEPROM.read #else #define EE_ADDR uint8_t* -#endif \ No newline at end of file +#endif diff --git a/Multiprotocol/Q303_nrf24l01.ino b/Multiprotocol/Q303_nrf24l01.ino index 54ba17e..a64334d 100644 --- a/Multiprotocol/Q303_nrf24l01.ino +++ b/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) { diff --git a/Multiprotocol/SFHSS_cc2500.ino b/Multiprotocol/SFHSS_cc2500.ino index 7f9dbd9..b336023 100644 --- a/Multiprotocol/SFHSS_cc2500.ino +++ b/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; diff --git a/Multiprotocol/SHENQI_nrf24l01.ino b/Multiprotocol/SHENQI_nrf24l01.ino index ca3d873..300eb20 100644 --- a/Multiprotocol/SHENQI_nrf24l01.ino +++ b/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); diff --git a/Multiprotocol/SLT_nrf24l01.ino b/Multiprotocol/SLT_nrf24l01.ino index 7e8affb..38bae28 100644 --- a/Multiprotocol/SLT_nrf24l01.ino +++ b/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; diff --git a/Multiprotocol/Symax_nrf24l01.ino b/Multiprotocol/Symax_nrf24l01.ino index f33a344..ce705c3 100644 --- a/Multiprotocol/Symax_nrf24l01.ino +++ b/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; } diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index f5b3b26..cded62f 100644 --- a/Multiprotocol/Telemetry.ino +++ b/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 diff --git a/Multiprotocol/V2X2_nrf24l01.ino b/Multiprotocol/V2X2_nrf24l01.ino index 9a34185..bce24bd 100644 --- a/Multiprotocol/V2X2_nrf24l01.ino +++ b/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; } diff --git a/Multiprotocol/WK2x01_cyrf6936.ino b/Multiprotocol/WK2x01_cyrf6936.ino index 614a67a..0404bc9 100644 --- a/Multiprotocol/WK2x01_cyrf6936.ino +++ b/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) diff --git a/Multiprotocol/YD717_nrf24l01.ino b/Multiprotocol/YD717_nrf24l01.ino index 2fdeda4..f4a3e5b 100644 --- a/Multiprotocol/YD717_nrf24l01.ino +++ b/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) { @@ -185,4 +186,4 @@ uint16_t initYD717() return YD717_INITIAL_WAIT; } -#endif \ No newline at end of file +#endif