From 6d38dd2d7a74cdac855a766dc279474efb2ffe3e Mon Sep 17 00:00:00 2001 From: Pascal Langer Date: Tue, 15 Dec 2020 23:58:56 +0100 Subject: [PATCH] WFLY2: auto stop bind when the RX replies --- Multiprotocol/A7105_SPI.ino | 2 +- Multiprotocol/Multiprotocol.h | 2 +- Multiprotocol/WFLY2_a7105.ino | 253 ++++++++++++++++------------------ 3 files changed, 119 insertions(+), 138 deletions(-) diff --git a/Multiprotocol/A7105_SPI.ino b/Multiprotocol/A7105_SPI.ino index 91b5401..38950f6 100644 --- a/Multiprotocol/A7105_SPI.ino +++ b/Multiprotocol/A7105_SPI.ino @@ -27,7 +27,7 @@ void A7105_WriteData(uint8_t len, uint8_t channel) for (i = 0; i < len; i++) SPI_Write(packet[i]); A7105_CSN_on; - if(protocol!=PROTO_WFLY2 || phase==0) + if(protocol!=PROTO_WFLY2) { if(!(protocol==PROTO_FLYSKY || protocol==PROTO_KYOSHO)) { diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index f9aa271..e33e043 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 3 #define VERSION_REVISION 1 -#define VERSION_PATCH_LEVEL 85 +#define VERSION_PATCH_LEVEL 86 //****************** // Protocols diff --git a/Multiprotocol/WFLY2_a7105.ino b/Multiprotocol/WFLY2_a7105.ino index 9cc674a..ca8fe47 100644 --- a/Multiprotocol/WFLY2_a7105.ino +++ b/Multiprotocol/WFLY2_a7105.ino @@ -20,107 +20,100 @@ //#define WFLY2_FORCE_ID //WFLY2 constants & variables -#define WFLY2_BIND_COUNT 1000 +#define WFLY2_BIND_COUNT 2777 // abort bind after 10sec #define WFLY2_PACKET_SIZE 32 enum{ - WFLY2_BIND, WFLY2_DATA, WFLY2_PLL_TX, WFLY2_RX, }; -static void __attribute__((unused)) WFLY2_send_bind_packet() -{ - //Header - packet[0] = 0x0F; // Bind packet - - //ID - packet[1] = rx_tx_addr[3]; - packet[2] = rx_tx_addr[2]; - packet[3] = rx_tx_addr[1]; - - //Unknown - packet[4] = 0x00; - packet[5] = 0x01; - - //Freq - rf_ch_num = (hopping_frequency_no<<1)+0x08; - packet[6] = rf_ch_num; - rf_ch_num = (rf_ch_num<<1)+0x10; - hopping_frequency_no++; - if(hopping_frequency_no > 0x17) hopping_frequency_no=0x00; - - //Unknown - memset(&packet[7],0x00,25); - - //Debug - #if 0 - debug("ch=%02X P=",rf_ch_num); - for(uint8_t i=0; i>8; - - //RF channel - int8_t prev = rf_ch_num & 0x1F; - rf_ch_num = (pseudo ^ (pseudo >> 7)) & 0x57; - if(rf_ch_num & 0x10) + memset(packet,0x00,WFLY2_PACKET_SIZE); + + if(IS_BIND_IN_PROGRESS) { - rf_ch_num |= 0x08; - rf_ch_num &= 0x4F; - } - if(rf_ch_num & 0x40) - { - rf_ch_num |= 0x10; - rf_ch_num &= 0x1F; - } - rf_ch_num ^= rx_tx_addr[3] & 0x1F; - if(abs((int8_t)rf_ch_num-prev) <= 9) - { - if(high_bit) - rf_ch_num |= 0x20; + //Header + packet[0] = 0x0F; // Bind packet + + //ID + packet[1] = rx_tx_addr[3]; + packet[2] = rx_tx_addr[2]; + packet[3] = rx_tx_addr[1]; + //packet[4] = 0x00; // Should be rx_tx_addr[0]&0x0F but bind is already using 0x00 so .... + + //Unknown + packet[5] = 0x01; + + //Freq + rf_ch_num = (hopping_frequency_no<<1)+0x08; + packet[6] = rf_ch_num; + hopping_frequency_no++; + if(hopping_frequency_no > 0x17) hopping_frequency_no=0x00; + + //Unknown bytes 7..31 } else - if(!high_bit) - rf_ch_num |= 0x20; - - //Partial ID - packet[3] = rx_tx_addr[3]; - packet[4] = rx_tx_addr[2] & 0x03; - - //10 channels -100%=0x2C1...0%=0x800...+100%=0xD3F - for(uint8_t i = 0; i < 5; i++) { - uint16_t temp=convert_channel_16b_nolimit(i*2 , 0x2C1, 0xD3F); - packet[5 + i*3] = temp&0xFF; // low byte - packet[7 + i*3] = (temp>>8)&0x0F; // high byte - temp=convert_channel_16b_nolimit(i*2+1, 0x2C1, 0xD3F); - packet[6 + i*3] = temp&0xFF; // low byte - packet[7 + i*3] |= (temp>>4)&0xF0; // high byte + //Header + //packet[0] = 0x00; // Normal packet + + //Pseudo + uint16_t high_bit=(pseudo & 0x8000) ^ 0x8000; // toggle 0x8000 every other line + pseudo <<= 1; // *2 + if( (pseudo & 0x8000) || pseudo == 0 ) pseudo ^= 0x8A87; // Randomisation, pseudo==0 is a guess but would give the start value seen on the dump when P[2]P[1]=0 at init and will prevent a lock up + pseudo |= high_bit; // include toggle + packet[1] = pseudo; + packet[2] = pseudo>>8; + + //RF channel + int8_t prev = rf_ch_num & 0x1F; + rf_ch_num = (pseudo ^ (pseudo >> 7)); + rf_ch_num = ((rf_ch_num>>1)&0x08) | (rf_ch_num & 0x47); + rf_ch_num = ((rf_ch_num>>2)&0x10) | (rf_ch_num & 0x1F); + rf_ch_num ^= rx_tx_addr[3] & 0x1F; + if(abs((int8_t)rf_ch_num-prev) <= 9) + { + if(high_bit) + rf_ch_num |= 0x20; + } + else + if(!high_bit) + rf_ch_num |= 0x20; + + //Partial ID + packet[3] = rx_tx_addr[3]; + packet[4] = rx_tx_addr[2] & 0x03; + + //10 channels -100%=0x2C1...0%=0x800...+100%=0xD3F + for(uint8_t i = 0; i < 5; i++) + { + uint16_t temp=convert_channel_16b_nolimit(i*2 , 0x2C1, 0xD3F); + packet[5 + i*3] = temp&0xFF; // low byte + packet[7 + i*3] = (temp>>8)&0x0F; // high byte + temp=convert_channel_16b_nolimit(i*2+1, 0x2C1, 0xD3F); + packet[6 + i*3] = temp&0xFF; // low byte + packet[7 + i*3] |= (temp>>4)&0xF0; // high byte + } + + //Unknown bytes 20..31 } - - //Unknown - memset(&packet[20],0x00,12); //Debug #if 0 @@ -135,7 +128,7 @@ static void __attribute__((unused)) WFLY2_build_packet() static void __attribute__((unused)) WFLY2_Send_Telemetry() { //Incoming packet values - v_lipo1=packet[3]<<1; // RX_batt*10 in V + v_lipo1=packet[3]<<1; // RX_batt *10 in V v_lipo2=packet[5]<<1; // Ext_batt*10 in V RX_RSSI=(255-packet[7])>>1; // Looks to be the RX RSSI value direct from A7105 @@ -160,30 +153,13 @@ static void __attribute__((unused)) WFLY2_build_packet() uint16_t ReadWFLY2() { uint16_t start; - #ifdef WFLY2_HUB_TELEMETRY - uint8_t status; - #endif + uint8_t status; #ifndef FORCE_WFLY2_TUNING A7105_AdjustLOBaseFreq(1); #endif switch(phase) { - case WFLY2_BIND: - bind_counter--; - if (bind_counter) - { - WFLY2_send_bind_packet(); - return WFLY2_PACKET_PERIOD; - } - else - { - BIND_DONE; - A7105_WriteID(MProtocol_id); - rf_ch_num = 0; - phase++; // WFLY2_DATA - } - case WFLY2_DATA: #ifdef MULTI_SYNC telemetry_set_input_sync(WFLY2_PACKET_PERIOD); @@ -211,34 +187,44 @@ uint16_t ReadWFLY2() return WFLY2_BUFFER_TIME; case WFLY2_PLL_TX: - #ifdef WFLY2_HUB_TELEMETRY - //Check RX status - status=A7105_ReadReg(A7105_00_MODE); - //debugln("S:%02X", status); - #endif + //Check RX status + status=A7105_ReadReg(A7105_00_MODE); + //debugln("S:%02X", status); //PLL A7105_Strobe(A7105_PLL); - #ifdef WFLY2_HUB_TELEMETRY - //Read incoming packet even if bad/not present to not change too much the TX timing, might want to reorg the code... - A7105_ReadData(WFLY2_PACKET_SIZE); + //Read incoming packet even if bad/not present to not change too much the TX timing, might want to reorg the code... + A7105_ReadData(WFLY2_PACKET_SIZE); - //Read telemetry - if((status & 0x21)==0) - { // Packet received and CRC OK - //Debug - #if 1 - debug("T:"); - for(uint8_t i=0; i