diff --git a/Multiprotocol/Bayang_nrf24l01.ino b/Multiprotocol/Bayang_nrf24l01.ino index 782d3ad..84907b4 100644 --- a/Multiprotocol/Bayang_nrf24l01.ino +++ b/Multiprotocol/Bayang_nrf24l01.ino @@ -38,7 +38,7 @@ enum BAYANG_FLAGS { BAYANG_FLAG_INVERTED = 0x80 // inverted flight on Floureon H101 }; -static void BAYANG_send_packet(uint8_t bind) +static void __attribute__((unused)) BAYANG_send_packet(uint8_t bind) { uint8_t i; if (bind) @@ -112,7 +112,7 @@ static void BAYANG_send_packet(uint8_t bind) NRF24L01_SetPower(); // Set tx_power } -static void BAYANG_init() +static void __attribute__((unused)) BAYANG_init() { NRF24L01_Initialize(); NRF24L01_SetTxRxMode(TX_EN); @@ -148,7 +148,7 @@ uint16_t BAYANG_callback() return BAYANG_PACKET_PERIOD; } -static void BAYANG_initialize_txid() +static void __attribute__((unused)) BAYANG_initialize_txid() { //Could be using txid[0..2] but using rx_tx_addr everywhere instead... hopping_frequency[0]=0; diff --git a/Multiprotocol/CG023_nrf24l01.ino b/Multiprotocol/CG023_nrf24l01.ino index adb6539..6810652 100644 --- a/Multiprotocol/CG023_nrf24l01.ino +++ b/Multiprotocol/CG023_nrf24l01.ino @@ -70,7 +70,7 @@ enum H8_3D_FLAGS_2 { H8_3D_FLAG_CALIBRATE = 0x20, // accelerometer calibration }; -static void CG023_send_packet(uint8_t bind) +static void __attribute__((unused)) CG023_send_packet(uint8_t bind) { // throttle : 0x00 - 0xFF throttle=convert_channel_8b(THROTTLE); @@ -205,7 +205,7 @@ static void CG023_send_packet(uint8_t bind) NRF24L01_SetPower(); // Set tx_power } -static void CG023_init() +static void __attribute__((unused)) CG023_init() { NRF24L01_Initialize(); NRF24L01_SetTxRxMode(TX_EN); @@ -246,7 +246,7 @@ uint16_t CG023_callback() return H8_3D_PACKET_PERIOD; } -static void CG023_initialize_txid() +static void __attribute__((unused)) CG023_initialize_txid() { if(sub_protocol==H8_3D) { diff --git a/Multiprotocol/CX10_nrf24l01.ino b/Multiprotocol/CX10_nrf24l01.ino index ff7073b..044c20c 100644 --- a/Multiprotocol/CX10_nrf24l01.ino +++ b/Multiprotocol/CX10_nrf24l01.ino @@ -46,7 +46,7 @@ enum { CX10_DATA }; -static void CX10_Write_Packet(uint8_t bind) +static void __attribute__((unused)) CX10_Write_Packet(uint8_t bind) { uint8_t offset = 0; if(sub_protocol == CX10_BLUE) @@ -166,7 +166,7 @@ static void CX10_Write_Packet(uint8_t bind) NRF24L01_SetPower(); } -static void CX10_init() +static void __attribute__((unused)) CX10_init() { NRF24L01_Initialize(); NRF24L01_SetTxRxMode(TX_EN); @@ -226,7 +226,7 @@ uint16_t CX10_callback() { return packet_period; } -static void initialize_txid() +static void __attribute__((unused)) initialize_txid() { rx_tx_addr[1]%= 0x30; if(sub_protocol==Q282) diff --git a/Multiprotocol/DSM2_cyrf6936.ino b/Multiprotocol/DSM2_cyrf6936.ino index ab1e781..e6769a6 100644 --- a/Multiprotocol/DSM2_cyrf6936.ino +++ b/Multiprotocol/DSM2_cyrf6936.ino @@ -120,7 +120,7 @@ const uint8_t cyrfmfg_id[6] = {0xd4, 0x62, 0xd6, 0xad, 0xd3, 0xff}; //dx6i #endif */ -static void build_bind_packet() +static void __attribute__((unused)) build_bind_packet() { uint8_t i; uint16_t sum = 384 - 0x10;// @@ -153,7 +153,23 @@ static void build_bind_packet() packet[15] = sum & 0xff; } -static void build_data_packet(uint8_t upper)// +static uint8_t __attribute__((unused)) PROTOCOL_SticksMoved(uint8_t init) +{ +#define STICK_MOVEMENT 15*(PPM_MAX-PPM_MIN)/100 // defines when the bind dialog should be interrupted (stick movement STICK_MOVEMENT %) + static uint16_t ele_start, ail_start; + uint16_t ele = Servo_data[ELEVATOR];//CHAN_ReadInput(MIXER_MapChannel(INP_ELEVATOR)); + uint16_t ail = Servo_data[AILERON];//CHAN_ReadInput(MIXER_MapChannel(INP_AILERON)); + if(init) { + ele_start = ele; + ail_start = ail; + return 0; + } + uint16_t ele_diff = ele_start - ele;//abs(ele_start - ele); + uint16_t ail_diff = ail_start - ail;//abs(ail_start - ail); + return ((ele_diff + ail_diff) > STICK_MOVEMENT);// +} + +static void __attribute__((unused)) build_data_packet(uint8_t upper)// { #if DSM2_NUM_CHANNELS==4 const uint8_t ch_map[] = {0, 1, 2, 3, 0xff, 0xff, 0xff}; //Guess @@ -251,23 +267,7 @@ static void build_data_packet(uint8_t upper)// } } -static uint8_t PROTOCOL_SticksMoved(uint8_t init) -{ -#define STICK_MOVEMENT 15*(PPM_MAX-PPM_MIN)/100 // defines when the bind dialog should be interrupted (stick movement STICK_MOVEMENT %) - static uint16_t ele_start, ail_start; - uint16_t ele = Servo_data[ELEVATOR];//CHAN_ReadInput(MIXER_MapChannel(INP_ELEVATOR)); - uint16_t ail = Servo_data[AILERON];//CHAN_ReadInput(MIXER_MapChannel(INP_AILERON)); - if(init) { - ele_start = ele; - ail_start = ail; - return 0; - } - uint16_t ele_diff = ele_start - ele;//abs(ele_start - ele); - uint16_t ail_diff = ail_start - ail;//abs(ail_start - ail); - return ((ele_diff + ail_diff) > STICK_MOVEMENT);// -} - -static uint8_t get_pn_row(uint8_t channel) +static uint8_t __attribute__((unused)) get_pn_row(uint8_t channel) { return (sub_protocol == DSMX ? (channel - 2) % 5 : channel % 5); } @@ -299,7 +299,7 @@ const uint8_t init_vals[][2] = { {CYRF_01_TX_LENGTH, 0x10}, //16byte packet }; -static void cyrf_config() +static void __attribute__((unused)) cyrf_config() { for(uint8_t i = 0; i < sizeof(init_vals) / 2; i++) CYRF_WriteRegister(init_vals[i][0], init_vals[i][1]); @@ -307,7 +307,7 @@ static void cyrf_config() CYRF_ConfigRFChannel(0x61); } -static void initialize_bind_state() +static void __attribute__((unused)) initialize_bind_state() { const uint8_t pn_bind[] = { 0xc6,0x94,0x22,0xfe,0x48,0xe6,0x57,0x4e }; uint8_t data_code[32]; @@ -341,13 +341,13 @@ const uint8_t data_vals[][2] = { {CYRF_10_FRAMING_CFG, 0xea}, }; -static void cyrf_configdata() +static void __attribute__((unused)) cyrf_configdata() { for(uint8_t i = 0; i < sizeof(data_vals) / 2; i++) CYRF_WriteRegister(data_vals[i][0], data_vals[i][1]); } -static void set_sop_data_crc() +static void __attribute__((unused)) set_sop_data_crc() { uint8_t pn_row = get_pn_row(hopping_frequency[chidx]); //printf("Ch: %d Row: %d SOP: %d Data: %d\n", ch[chidx], pn_row, sop_col, data_col); @@ -362,7 +362,7 @@ static void set_sop_data_crc() crcidx = !crcidx; } -static void calc_dsmx_channel() +static void __attribute__((unused)) calc_dsmx_channel() { uint8_t idx = 0; uint32_t id = ~(((uint32_t)cyrfmfg_id[0] << 24) | ((uint32_t)cyrfmfg_id[1] << 16) | ((uint32_t)cyrfmfg_id[2] << 8) | (cyrfmfg_id[3] << 0)); diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino index 3e8ec7a..e569cb9 100644 --- a/Multiprotocol/Devo_cyrf6936.ino +++ b/Multiprotocol/Devo_cyrf6936.ino @@ -66,7 +66,7 @@ uint8_t ch_idx; uint8_t use_fixed_id; uint8_t failsafe_pkt; -static void scramble_pkt() +static void __attribute__((unused)) scramble_pkt() { #ifdef NO_SCRAMBLE return; @@ -77,7 +77,7 @@ static void scramble_pkt() #endif } -static void add_pkt_suffix() +static void __attribute__((unused)) add_pkt_suffix() { uint8_t bind_state; if (use_fixed_id) @@ -97,7 +97,7 @@ static void add_pkt_suffix() packet[15] = (fixed_id >> 16) & 0xff; } -static void build_beacon_pkt(uint8_t upper) +static void __attribute__((unused)) build_beacon_pkt(uint8_t upper) { packet[0] = ((DEVO_NUM_CHANNELS << 4) | 0x07); // uint8_t enable = 0; @@ -116,7 +116,7 @@ static void build_beacon_pkt(uint8_t upper) add_pkt_suffix(); } -static void build_bind_pkt() +static void __attribute__((unused)) build_bind_pkt() { packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x0a; packet[1] = bind_counter & 0xff; @@ -136,7 +136,7 @@ static void build_bind_pkt() packet[15] ^= cyrfmfg_id[2]; } -static void build_data_pkt() +static void __attribute__((unused)) build_data_pkt() { uint8_t i; packet[0] = (DEVO_NUM_CHANNELS << 4) | (0x0b + ch_idx); @@ -161,7 +161,7 @@ static void build_data_pkt() add_pkt_suffix(); } -static void cyrf_set_bound_sop_code() +static void __attribute__((unused)) cyrf_set_bound_sop_code() { /* crc == 0 isn't allowed, so use 1 if the math results in 0 */ uint8_t crc = (cyrfmfg_id[0] + (cyrfmfg_id[1] >> 6) + cyrfmfg_id[2]); @@ -174,7 +174,7 @@ static void cyrf_set_bound_sop_code() CYRF_SetPower(0x08); } -static void cyrf_init() +static void __attribute__((unused)) cyrf_init() { /* Initialise CYRF chip */ CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x39); @@ -201,7 +201,7 @@ static void cyrf_init() CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28); } -static void set_radio_channels() +static void __attribute__((unused)) set_radio_channels() { //int i; CYRF_FindBestChannels(hopping_frequency, 3, 4, 4, 80); @@ -217,7 +217,7 @@ static void set_radio_channels() hopping_frequency[4] = hopping_frequency[1]; } -static void DEVO_BuildPacket() +static void __attribute__((unused)) DEVO_BuildPacket() { switch(phase) { @@ -302,7 +302,7 @@ uint16_t devo_callback() return 1200; } -/*static void devo_bind() +/*static void __attribute__((unused)) devo_bind() { fixed_id = Model_fixed_id; bind_counter = DEVO_BIND_COUNT; @@ -311,7 +311,7 @@ uint16_t devo_callback() } -static void generate_fixed_id_bind(){ +static void __attribute__((unused)) generate_fixed_id_bind(){ if(BIND_0){ //randomSeed((uint32_t)analogRead(A6)<<10|analogRead(A7));//seed uint8_t txid[4]; diff --git a/Multiprotocol/ESky_nrf24l01.ino b/Multiprotocol/ESky_nrf24l01.ino index f83128f..1ed7d3d 100644 --- a/Multiprotocol/ESky_nrf24l01.ino +++ b/Multiprotocol/ESky_nrf24l01.ino @@ -23,14 +23,14 @@ #define ESKY_PAYLOAD_SIZE 13 #define ESKY_PACKET_CHKTIME 100 // Time to wait for packet to be sent (no ACK, so very short) -static void ESKY_set_data_address() +static void __attribute__((unused)) ESKY_set_data_address() { NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x02); // 4-byte RX/TX address for regular packets NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 4); NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 4); } -static void ESKY_init(uint8_t bind) +static void __attribute__((unused)) ESKY_init(uint8_t bind) { NRF24L01_Initialize(); @@ -60,7 +60,7 @@ static void ESKY_init(uint8_t bind) NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here } -static void ESKY_init2() +static void __attribute__((unused)) ESKY_init2() { NRF24L01_FlushTx(); packet_sent = 0; @@ -90,7 +90,7 @@ static void ESKY_init2() NRF24L01_SetTxRxMode(TX_EN); } -static void ESKY_send_packet(uint8_t bind) +static void __attribute__((unused)) ESKY_send_packet(uint8_t bind) { uint8_t rf_ch = 50; // bind channel if (bind) diff --git a/Multiprotocol/FlySky_a7105.ino b/Multiprotocol/FlySky_a7105.ino index 452b5ff..8c5f956 100644 --- a/Multiprotocol/FlySky_a7105.ino +++ b/Multiprotocol/FlySky_a7105.ino @@ -73,7 +73,7 @@ uint8_t chanrow; uint8_t chancol; uint8_t chanoffset; -static void flysky_apply_extension_flags() +static void __attribute__((unused)) flysky_apply_extension_flags() { const uint8_t V912_X17_SEQ[10] = { 0x14, 0x31, 0x40, 0x49, 0x49, // sometime first byte is 0x15 ? 0x49, 0x49, 0x49, 0x49, 0x49, }; @@ -148,7 +148,7 @@ static void flysky_apply_extension_flags() } } -static void flysky_build_packet(uint8_t init) +static void __attribute__((unused)) flysky_build_packet(uint8_t init) { uint8_t i; //servodata timing range for flysky. diff --git a/Multiprotocol/FrSky_cc2500.ino b/Multiprotocol/FrSky_cc2500.ino index 5f90a46..449f954 100644 --- a/Multiprotocol/FrSky_cc2500.ino +++ b/Multiprotocol/FrSky_cc2500.ino @@ -34,6 +34,135 @@ enum { }; */ +static void __attribute__((unused)) frsky2way_init(uint8_t bind) +{ + // Configure cc2500 for tx mode + CC2500_Reset(); + // + cc2500_writeReg(CC2500_02_IOCFG0, 0x06); + cc2500_writeReg(CC2500_00_IOCFG2, 0x06); + cc2500_writeReg(CC2500_17_MCSM1, 0x0c); + cc2500_writeReg(CC2500_18_MCSM0, 0x18); + cc2500_writeReg(CC2500_06_PKTLEN, 0x19); + cc2500_writeReg(CC2500_07_PKTCTRL1, 0x04); + cc2500_writeReg(CC2500_08_PKTCTRL0, 0x05); + cc2500_writeReg(CC2500_3E_PATABLE, 0xff); + cc2500_writeReg(CC2500_0B_FSCTRL1, 0x08); + cc2500_writeReg(CC2500_0C_FSCTRL0, option); + //base freq FREQ = 0x5C7627 (F = 2404MHz) + cc2500_writeReg(CC2500_0D_FREQ2, 0x5c); + cc2500_writeReg(CC2500_0E_FREQ1, 0x76); + cc2500_writeReg(CC2500_0F_FREQ0, 0x27); + // + cc2500_writeReg(CC2500_10_MDMCFG4, 0xAA); + cc2500_writeReg(CC2500_11_MDMCFG3, 0x39); + cc2500_writeReg(CC2500_12_MDMCFG2, 0x11); + cc2500_writeReg(CC2500_13_MDMCFG1, 0x23); + cc2500_writeReg(CC2500_14_MDMCFG0, 0x7a); + cc2500_writeReg(CC2500_15_DEVIATN, 0x42); + cc2500_writeReg(CC2500_19_FOCCFG, 0x16); + cc2500_writeReg(CC2500_1A_BSCFG, 0x6c); + cc2500_writeReg(CC2500_1B_AGCCTRL2, bind ? 0x43 : 0x03); + cc2500_writeReg(CC2500_1C_AGCCTRL1,0x40); + cc2500_writeReg(CC2500_1D_AGCCTRL0,0x91); + cc2500_writeReg(CC2500_21_FREND1, 0x56); + cc2500_writeReg(CC2500_22_FREND0, 0x10); + cc2500_writeReg(CC2500_23_FSCAL3, 0xa9); + cc2500_writeReg(CC2500_24_FSCAL2, 0x0A); + cc2500_writeReg(CC2500_25_FSCAL1, 0x00); + cc2500_writeReg(CC2500_26_FSCAL0, 0x11); + cc2500_writeReg(CC2500_29_FSTEST, 0x59); + cc2500_writeReg(CC2500_2C_TEST2, 0x88); + cc2500_writeReg(CC2500_2D_TEST1, 0x31); + cc2500_writeReg(CC2500_2E_TEST0, 0x0B); + cc2500_writeReg(CC2500_03_FIFOTHR, 0x07); + cc2500_writeReg(CC2500_09_ADDR, 0x00); + // + CC2500_SetTxRxMode(TX_EN); + CC2500_SetPower(); + + cc2500_strobe(CC2500_SIDLE); + + cc2500_writeReg(CC2500_09_ADDR, bind ? 0x03 : rx_tx_addr[3]); + cc2500_writeReg(CC2500_07_PKTCTRL1, 0x05); + cc2500_strobe(CC2500_SIDLE); // Go to idle... + // + cc2500_writeReg(CC2500_0A_CHANNR, 0x00); + cc2500_writeReg(CC2500_23_FSCAL3, 0x89); + cc2500_strobe(CC2500_SFRX); + //#######END INIT######## +} + +static uint8_t __attribute__((unused)) get_chan_num(uint16_t idx) +{ + uint8_t ret = (idx * 0x1e) % 0xeb; + if(idx == 3 || idx == 23 || idx == 47) + ret++; + if(idx > 47) + return 0; + return ret; +} + +static void __attribute__((unused)) frsky2way_build_bind_packet() +{ + //11 03 01 d7 2d 00 00 1e 3c 5b 78 00 00 00 00 00 00 01 + //11 03 01 19 3e 00 02 8e 2f bb 5c 00 00 00 00 00 00 01 + packet[0] = 0x11; + packet[1] = 0x03; + packet[2] = 0x01; + packet[3] = rx_tx_addr[3]; + packet[4] = rx_tx_addr[2]; + uint16_t idx = ((state -FRSKY_BIND) % 10) * 5; + packet[5] = idx; + packet[6] = get_chan_num(idx++); + packet[7] = get_chan_num(idx++); + packet[8] = get_chan_num(idx++); + packet[9] = get_chan_num(idx++); + packet[10] = get_chan_num(idx++); + packet[11] = 0x00; + packet[12] = 0x00; + packet[13] = 0x00; + packet[14] = 0x00; + packet[15] = 0x00; + packet[16] = 0x00; + packet[17] = 0x01; +} + + + +static void __attribute__((unused)) frsky2way_data_frame() +{//pachet[4] is telemetry user frame counter(hub) + //11 d7 2d 22 00 01 c9 c9 ca ca 88 88 ca ca c9 ca 88 88 + //11 57 12 00 00 01 f2 f2 f2 f2 06 06 ca ca ca ca 18 18 + packet[0] = 0x11; //Length + packet[1] = rx_tx_addr[3]; + packet[2] = rx_tx_addr[2]; + packet[3] = counter;// + packet[4]=telemetry_counter; + + packet[5] = 0x01; + // + packet[10] = 0; + packet[11] = 0; + packet[16] = 0; + packet[17] = 0; + for(uint8_t i = 0; i < 8; i++) + { + uint16_t value; + value = convert_channel_frsky(i); + if(i < 4) + { + packet[6+i] = value & 0xff; + packet[10+(i>>1)] |= ((value >> 8) & 0x0f) << (4 *(i & 0x01)); + } + else + { + packet[8+i] = value & 0xff; + packet[16+((i-4)>>1)] |= ((value >> 8) & 0x0f) << (4 * ((i-4) & 0x01)); + } + } +} + uint16_t initFrSky_2way() { if(IS_AUTOBIND_FLAG_on) @@ -49,6 +178,26 @@ uint16_t initFrSky_2way() return 10000; } +#if defined(TELEMETRY) +static void __attribute__((unused)) check_telemetry(uint8_t *pkt,uint8_t len) +{ + if(pkt[1] != rx_tx_addr[3] || pkt[2] != rx_tx_addr[2] || len != pkt[0] + 3) + {//only packets with the required id and packet length + for(uint8_t i=3;i<6;i++) + pktt[i]=0; + return; + } + else + { + for (uint8_t i=3;i0) + telemetry_counter=(telemetry_counter+1)%32; + } +} +#endif + uint16_t ReadFrSky_2way() { if (state < FRSKY_BIND_DONE) @@ -112,163 +261,4 @@ uint16_t ReadFrSky_2way() } return state == FRSKY_DATA4 ? 7500 : 9000; } - -#if defined(TELEMETRY) -static void check_telemetry(uint8_t *pkt,uint8_t len) -{ - if(pkt[1] != rx_tx_addr[3] || pkt[2] != rx_tx_addr[2] || len != pkt[0] + 3) - {//only packets with the required id and packet length - for(uint8_t i=3;i<6;i++) - pktt[i]=0; - return; - } - else - { - for (uint8_t i=3;i0) - telemetry_counter=(telemetry_counter+1)%32; - } -} - -void compute_RSSIdbm(){ - RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>5); - if(pktt[len-2] >=128) - RSSI_dBm -= 82; - else - RSSI_dBm += 65; -} - -#endif - -static void frsky2way_init(uint8_t bind) -{ - // Configure cc2500 for tx mode - CC2500_Reset(); - // - cc2500_writeReg(CC2500_02_IOCFG0, 0x06); - cc2500_writeReg(CC2500_00_IOCFG2, 0x06); - cc2500_writeReg(CC2500_17_MCSM1, 0x0c); - cc2500_writeReg(CC2500_18_MCSM0, 0x18); - cc2500_writeReg(CC2500_06_PKTLEN, 0x19); - cc2500_writeReg(CC2500_07_PKTCTRL1, 0x04); - cc2500_writeReg(CC2500_08_PKTCTRL0, 0x05); - cc2500_writeReg(CC2500_3E_PATABLE, 0xff); - cc2500_writeReg(CC2500_0B_FSCTRL1, 0x08); - cc2500_writeReg(CC2500_0C_FSCTRL0, option); - //base freq FREQ = 0x5C7627 (F = 2404MHz) - cc2500_writeReg(CC2500_0D_FREQ2, 0x5c); - cc2500_writeReg(CC2500_0E_FREQ1, 0x76); - cc2500_writeReg(CC2500_0F_FREQ0, 0x27); - // - cc2500_writeReg(CC2500_10_MDMCFG4, 0xAA); - cc2500_writeReg(CC2500_11_MDMCFG3, 0x39); - cc2500_writeReg(CC2500_12_MDMCFG2, 0x11); - cc2500_writeReg(CC2500_13_MDMCFG1, 0x23); - cc2500_writeReg(CC2500_14_MDMCFG0, 0x7a); - cc2500_writeReg(CC2500_15_DEVIATN, 0x42); - cc2500_writeReg(CC2500_19_FOCCFG, 0x16); - cc2500_writeReg(CC2500_1A_BSCFG, 0x6c); - cc2500_writeReg(CC2500_1B_AGCCTRL2, bind ? 0x43 : 0x03); - cc2500_writeReg(CC2500_1C_AGCCTRL1,0x40); - cc2500_writeReg(CC2500_1D_AGCCTRL0,0x91); - cc2500_writeReg(CC2500_21_FREND1, 0x56); - cc2500_writeReg(CC2500_22_FREND0, 0x10); - cc2500_writeReg(CC2500_23_FSCAL3, 0xa9); - cc2500_writeReg(CC2500_24_FSCAL2, 0x0A); - cc2500_writeReg(CC2500_25_FSCAL1, 0x00); - cc2500_writeReg(CC2500_26_FSCAL0, 0x11); - cc2500_writeReg(CC2500_29_FSTEST, 0x59); - cc2500_writeReg(CC2500_2C_TEST2, 0x88); - cc2500_writeReg(CC2500_2D_TEST1, 0x31); - cc2500_writeReg(CC2500_2E_TEST0, 0x0B); - cc2500_writeReg(CC2500_03_FIFOTHR, 0x07); - cc2500_writeReg(CC2500_09_ADDR, 0x00); - // - CC2500_SetTxRxMode(TX_EN); - CC2500_SetPower(); - - cc2500_strobe(CC2500_SIDLE); - - cc2500_writeReg(CC2500_09_ADDR, bind ? 0x03 : rx_tx_addr[3]); - cc2500_writeReg(CC2500_07_PKTCTRL1, 0x05); - cc2500_strobe(CC2500_SIDLE); // Go to idle... - // - cc2500_writeReg(CC2500_0A_CHANNR, 0x00); - cc2500_writeReg(CC2500_23_FSCAL3, 0x89); - cc2500_strobe(CC2500_SFRX); - //#######END INIT######## -} - -static uint8_t get_chan_num(uint16_t idx) -{ - uint8_t ret = (idx * 0x1e) % 0xeb; - if(idx == 3 || idx == 23 || idx == 47) - ret++; - if(idx > 47) - return 0; - return ret; -} - -static void frsky2way_build_bind_packet() -{ - //11 03 01 d7 2d 00 00 1e 3c 5b 78 00 00 00 00 00 00 01 - //11 03 01 19 3e 00 02 8e 2f bb 5c 00 00 00 00 00 00 01 - packet[0] = 0x11; - packet[1] = 0x03; - packet[2] = 0x01; - packet[3] = rx_tx_addr[3]; - packet[4] = rx_tx_addr[2]; - uint16_t idx = ((state -FRSKY_BIND) % 10) * 5; - packet[5] = idx; - packet[6] = get_chan_num(idx++); - packet[7] = get_chan_num(idx++); - packet[8] = get_chan_num(idx++); - packet[9] = get_chan_num(idx++); - packet[10] = get_chan_num(idx++); - packet[11] = 0x00; - packet[12] = 0x00; - packet[13] = 0x00; - packet[14] = 0x00; - packet[15] = 0x00; - packet[16] = 0x00; - packet[17] = 0x01; -} - - - -static void frsky2way_data_frame() -{//pachet[4] is telemetry user frame counter(hub) - //11 d7 2d 22 00 01 c9 c9 ca ca 88 88 ca ca c9 ca 88 88 - //11 57 12 00 00 01 f2 f2 f2 f2 06 06 ca ca ca ca 18 18 - packet[0] = 0x11; //Length - packet[1] = rx_tx_addr[3]; - packet[2] = rx_tx_addr[2]; - packet[3] = counter;// - packet[4]=telemetry_counter; - - packet[5] = 0x01; - // - packet[10] = 0; - packet[11] = 0; - packet[16] = 0; - packet[17] = 0; - for(uint8_t i = 0; i < 8; i++) - { - uint16_t value; - value = convert_channel_frsky(i); - if(i < 4) - { - packet[6+i] = value & 0xff; - packet[10+(i>>1)] |= ((value >> 8) & 0x0f) << (4 *(i & 0x01)); - } - else - { - packet[8+i] = value & 0xff; - packet[16+((i-4)>>1)] |= ((value >> 8) & 0x0f) << (4 * ((i-4) & 0x01)); - } - } -} - #endif diff --git a/Multiprotocol/Hisky_nrf24l01.ino b/Multiprotocol/Hisky_nrf24l01.ino index d413cbe..c6edabe 100644 --- a/Multiprotocol/Hisky_nrf24l01.ino +++ b/Multiprotocol/Hisky_nrf24l01.ino @@ -27,7 +27,7 @@ uint8_t bind_buf_arry[4][10]; // HiSky protocol uses TX id as an address for nRF24L01, and uses frequency hopping sequence // which does not depend on this id and is passed explicitly in binding sequence. So we are free // to generate this sequence as we wish. It should be in the range [02..77] -static void calc_fh_channels() +static void __attribute__((unused)) calc_fh_channels() { uint8_t idx = 0; uint32_t rnd = MProtocol_id; @@ -61,7 +61,7 @@ static void calc_fh_channels() } } -static void build_binding_packet(void) +static void __attribute__((unused)) build_binding_packet(void) { uint8_t i; uint16_t sum=0; @@ -95,7 +95,7 @@ static void build_binding_packet(void) } } -static void hisky_init() +static void __attribute__((unused)) hisky_init() { NRF24L01_Initialize(); @@ -116,7 +116,7 @@ static void hisky_init() // HiSky channel sequence: AILE ELEV THRO RUDD GEAR PITCH, channel data value is from 0 to 1000 // Channel 7 - Gyro mode, 0 - 6 axis, 3 - 3 axis -static void build_ch_data() +static void __attribute__((unused)) build_ch_data() { uint16_t temp; uint8_t i,j; @@ -217,7 +217,7 @@ uint16_t hisky_cb() return 1000; // send 1 binding packet and 1 data packet per 9ms } -static void initialize_tx_id() +static void __attribute__((unused)) initialize_tx_id() { //Generate frequency hopping table if(sub_protocol==HK310) diff --git a/Multiprotocol/Hubsan_a7105.ino b/Multiprotocol/Hubsan_a7105.ino index 6cd5c03..d13bdb7 100644 --- a/Multiprotocol/Hubsan_a7105.ino +++ b/Multiprotocol/Hubsan_a7105.ino @@ -56,7 +56,7 @@ enum { }; #define WAIT_WRITE 0x80 -static void update_crc() +static void __attribute__((unused)) hubsan_update_crc() { uint8_t sum = 0; for(uint8_t i = 0; i < 15; i++) @@ -64,7 +64,7 @@ static void update_crc() packet[15] = (256 - (sum % 256)) & 0xFF; } -static void hubsan_build_bind_packet(uint8_t bind_state) +static void __attribute__((unused)) hubsan_build_bind_packet(uint8_t bind_state) { static uint8_t handshake_counter; if(phase < BIND_7) @@ -99,14 +99,14 @@ static void hubsan_build_bind_packet(uint8_t bind_state) if(phase == BIND_7) packet[2] = handshake_counter++; } - update_crc(); + hubsan_update_crc(); } //cc : throttle observed range: 0x00 - 0xFF (smaller is down) //ee : rudder observed range: 0x34 - 0xcc (smaller is right)52-204-60% //gg : elevator observed range: 0x3e - 0xbc (smaller is up)62-188 -50% //ii : aileron observed range: 0x45 - 0xc3 (smaller is right)69-195-50% -static void hubsan_build_packet() +static void __attribute__((unused)) hubsan_build_packet() { static uint8_t vtx_freq = 0; memset(packet, 0, 16); @@ -177,7 +177,7 @@ static void hubsan_build_packet() packet_count++; } } - update_crc(); + hubsan_update_crc(); } #if defined(TELEMETRY) @@ -193,8 +193,10 @@ static void hubsan_build_packet() uint16_t ReadHubsan() { - static uint8_t txState=0; +#if defined(TELEMETRY) static uint8_t rfMode=0; +#endif + static uint8_t txState=0; static uint8_t bind_count=0; uint16_t delay; uint8_t i; @@ -277,7 +279,9 @@ uint16_t ReadHubsan() case DATA_4: case DATA_5: if( txState == 0) { // send packet +#if defined(TELEMETRY) rfMode = A7105_TX; +#endif if( phase == DATA_1) A7105_SetPower(); //Keep transmit power in sync hubsan_build_packet(); diff --git a/Multiprotocol/KN_nrf24l01.ino b/Multiprotocol/KN_nrf24l01.ino index 6c89ff1..53e902d 100644 --- a/Multiprotocol/KN_nrf24l01.ino +++ b/Multiprotocol/KN_nrf24l01.ino @@ -70,6 +70,205 @@ enum { KN_FLAG_GYROR = 0x80 // Always 0 so far }; +//------------------------------------------------------------------------------------------------- +// This function init 24L01 regs and packet data for binding +// Send tx address, hopping table (for Wl Toys), and data rate to the KN receiver during binding. +// It seems that KN can remember these parameters, no binding needed after power up. +// Bind uses fixed TX address "KNDZK", 1 Mbps data rate and channel 83 +//------------------------------------------------------------------------------------------------- +static void __attribute__((unused)) kn_bind_init() +{ + NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t*)"KNDZK", 5); + packet[0] = 'K'; + packet[1] = 'N'; + packet[2] = 'D'; + packet[3] = 'Z'; + //Use first four bytes of tx_addr + packet[4] = rx_tx_addr[0]; + packet[5] = rx_tx_addr[1]; + packet[6] = rx_tx_addr[2]; + packet[7] = rx_tx_addr[3]; + + if(sub_protocol==WLTOYS) + { + packet[8] = hopping_frequency[0]; + packet[9] = hopping_frequency[1]; + packet[10] = hopping_frequency[2]; + packet[11] = hopping_frequency[3]; + } + else + { + packet[8] = 0x00; + packet[9] = 0x00; + packet[10] = 0x00; + packet[11] = 0x00; + } + packet[12] = 0x00; + packet[13] = 0x00; + packet[14] = 0x00; + packet[15] = 0x01; //(USE1MBPS_YES) ? 0x01 : 0x00; + + //Set RF channel + NRF24L01_WriteReg(NRF24L01_05_RF_CH, 83); +} + +//------------------------------------------------------------------------------------------------- +// Update control data to be sent +// Do it once per frequency, so the same values will be sent 4 times +// KN uses 4 10-bit data channels plus a 8-bit switch channel +// +// The packet[0] is used for pitch/throttle, the relation is hard coded, not changeable. +// We can change the throttle/pitch range though. +// +// How to use trim? V977 stock controller can trim 6-axis mode to eliminate the drift. +//------------------------------------------------------------------------------------------------- +static void __attribute__((unused)) kn_update_packet_control_data() +{ + uint16_t value; + value = convert_channel_10b(THROTTLE); + packet[0] = (value >> 8) & 0xFF; + packet[1] = value & 0xFF; + value = convert_channel_10b(AILERON); + packet[2] = (value >> 8) & 0xFF; + packet[3] = value & 0xFF; + value = convert_channel_10b(ELEVATOR); + packet[4] = (value >> 8) & 0xFF; + packet[5] = value & 0xFF; + value = convert_channel_10b(RUDDER); + packet[6] = (value >> 8) & 0xFF; + packet[7] = value & 0xFF; + // Trims, middle is 0x64 (100) range 0-200 + packet[8] = convert_channel_8b_scale(AUX5,0,200); // 0x64; // T + packet[9] = convert_channel_8b_scale(AUX6,0,200); // 0x64; // A + packet[10] = convert_channel_8b_scale(AUX7,0,200); // 0x64; // E + packet[11] = 0x64; // R + + flags=0; + if (Servo_data[AUX1] > PPM_SWITCH) + flags = KN_FLAG_DR; + if (Servo_data[AUX2] > PPM_SWITCH) + flags |= KN_FLAG_TH; + if (Servo_data[AUX3] > PPM_SWITCH) + flags |= KN_FLAG_IDLEUP; + if (Servo_data[AUX4] > PPM_SWITCH) + flags |= KN_FLAG_GYRO3; + + packet[12] = flags; + + packet[13] = 0x00; + if(sub_protocol==WLTOYS) + packet[13] = (packet_sent << 5) | (hopping_frequency_no << 2); + + packet[14] = 0x00; + packet[15] = 0x00; + + NRF24L01_SetPower(); +} + + +//------------------------------------------------------------------------------------------------- +// This function generate RF TX packet address +// V977 can remember the binding parameters; we do not need rebind when power up. +// This requires the address must be repeatable for a specific RF ID at power up. +//------------------------------------------------------------------------------------------------- +static void __attribute__((unused)) kn_calculate_tx_addr() +{ + if(sub_protocol==FEILUN) + { + uint8_t addr2; + // Generate TXID with sum of minimum 256 and maximum 256+MAX_RF_CHANNEL-32 + rx_tx_addr[1] = 1 + rx_tx_addr[0] % (KN_MAX_RF_CHANNEL-33); + addr2 = 1 + rx_tx_addr[2] % (KN_MAX_RF_CHANNEL-33); + if ((uint16_t)(rx_tx_addr[0] + rx_tx_addr[1]) < 256) + rx_tx_addr[2] = addr2; + else + rx_tx_addr[2] = 0x00; + rx_tx_addr[3] = 0x00; + while((uint16_t)(rx_tx_addr[0] + rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3]) < 257) + rx_tx_addr[3] += addr2; + } + //The 5th byte is a constant, must be 'K' + rx_tx_addr[4] = 'K'; +} + +//------------------------------------------------------------------------------------------------- +// This function generates "random" RF hopping frequency channel numbers. +// These numbers must be repeatable for a specific seed +// The generated number range is from 0 to MAX_RF_CHANNEL. No repeat or adjacent numbers +// +// For Feilun variant, the channels are calculated from TXID, and since only 2 channels are used +// we copy them to fill up to MAX_RF_CHANNEL +//------------------------------------------------------------------------------------------------- +static void __attribute__((unused)) kn_calculate_freqency_hopping_channels() +{ + if(sub_protocol==WLTOYS) + { + uint8_t idx = 0; + uint32_t rnd = MProtocol_id; + while (idx < KN_RF_CH_COUNT) + { + uint8_t i; + rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization + + // Use least-significant byte. 73 is prime, so channels 76..77 are unused + uint8_t next_ch = ((rnd >> 8) % KN_MAX_RF_CHANNEL) + 2; + // Keep the distance 2 between the channels - either odd or even + if (((next_ch ^ MProtocol_id) & 0x01 )== 0) + continue; + // Check that it's not duplicate and spread uniformly + for (i = 0; i < idx; i++) + if(hopping_frequency[i] == next_ch) + break; + if (i != idx) + continue; + hopping_frequency[idx++] = next_ch; + } + } + else + {//FEILUN + hopping_frequency[0] = rx_tx_addr[0] + rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3]; // - 256; ??? + hopping_frequency[1] = hopping_frequency[0] + 32; + hopping_frequency[2] = hopping_frequency[0]; + hopping_frequency[3] = hopping_frequency[1]; + } +} + +//------------------------------------------------------------------------------------------------- +// This function setup 24L01 +// V977 uses one way communication, receiving only. 24L01 RX is never enabled. +// V977 needs payload length in the packet. We should configure 24L01 to enable Packet Control Field(PCF) +// Some RX reg settings are actually for enable PCF +//------------------------------------------------------------------------------------------------- +static void __attribute__((unused)) kn_init() +{ + kn_calculate_tx_addr(); + kn_calculate_freqency_hopping_channels(); + + NRF24L01_Initialize(); + + NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO)); + NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknoledgement + NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0 + NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address + NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0); // Disable retransmit + NRF24L01_SetPower(); + NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit + NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 0x20); // bytes of data payload for pipe 0 + + + NRF24L01_Activate(0x73); + NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 1); // Dynamic payload for data pipe 0 + // Enable: Dynamic Payload Length to enable PCF + NRF24L01_WriteReg(NRF24L01_1D_FEATURE, BV(NRF2401_1D_EN_DPL)); + + NRF24L01_SetPower(); + + NRF24L01_FlushTx(); + // Turn radio power on + NRF24L01_SetTxRxMode(TX_EN); + NRF24L01_SetBitrate(NRF24L01_BR_1M); //USE1MBPS_YES ? NRF24L01_BR_1M : NRF24L01_BR_250K; +} + //================================================================================================ // Private Functions //================================================================================================ @@ -145,202 +344,4 @@ uint16_t kn_callback() return packet_period; } -//------------------------------------------------------------------------------------------------- -// This function init 24L01 regs and packet data for binding -// Send tx address, hopping table (for Wl Toys), and data rate to the KN receiver during binding. -// It seems that KN can remember these parameters, no binding needed after power up. -// Bind uses fixed TX address "KNDZK", 1 Mbps data rate and channel 83 -//------------------------------------------------------------------------------------------------- -static void kn_bind_init() -{ - NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t*)"KNDZK", 5); - packet[0] = 'K'; - packet[1] = 'N'; - packet[2] = 'D'; - packet[3] = 'Z'; - //Use first four bytes of tx_addr - packet[4] = rx_tx_addr[0]; - packet[5] = rx_tx_addr[1]; - packet[6] = rx_tx_addr[2]; - packet[7] = rx_tx_addr[3]; - - if(sub_protocol==WLTOYS) - { - packet[8] = hopping_frequency[0]; - packet[9] = hopping_frequency[1]; - packet[10] = hopping_frequency[2]; - packet[11] = hopping_frequency[3]; - } - else - { - packet[8] = 0x00; - packet[9] = 0x00; - packet[10] = 0x00; - packet[11] = 0x00; - } - packet[12] = 0x00; - packet[13] = 0x00; - packet[14] = 0x00; - packet[15] = 0x01; //(USE1MBPS_YES) ? 0x01 : 0x00; - - //Set RF channel - NRF24L01_WriteReg(NRF24L01_05_RF_CH, 83); -} - -//------------------------------------------------------------------------------------------------- -// Update control data to be sent -// Do it once per frequency, so the same values will be sent 4 times -// KN uses 4 10-bit data channels plus a 8-bit switch channel -// -// The packet[0] is used for pitch/throttle, the relation is hard coded, not changeable. -// We can change the throttle/pitch range though. -// -// How to use trim? V977 stock controller can trim 6-axis mode to eliminate the drift. -//------------------------------------------------------------------------------------------------- -static void kn_update_packet_control_data() -{ - uint16_t value; - value = convert_channel_10b(THROTTLE); - packet[0] = (value >> 8) & 0xFF; - packet[1] = value & 0xFF; - value = convert_channel_10b(AILERON); - packet[2] = (value >> 8) & 0xFF; - packet[3] = value & 0xFF; - value = convert_channel_10b(ELEVATOR); - packet[4] = (value >> 8) & 0xFF; - packet[5] = value & 0xFF; - value = convert_channel_10b(RUDDER); - packet[6] = (value >> 8) & 0xFF; - packet[7] = value & 0xFF; - // Trims, middle is 0x64 (100) range 0-200 - packet[8] = convert_channel_8b_scale(AUX5,0,200); // 0x64; // T - packet[9] = convert_channel_8b_scale(AUX6,0,200); // 0x64; // A - packet[10] = convert_channel_8b_scale(AUX7,0,200); // 0x64; // E - packet[11] = 0x64; // R - - flags=0; - if (Servo_data[AUX1] > PPM_SWITCH) - flags = KN_FLAG_DR; - if (Servo_data[AUX2] > PPM_SWITCH) - flags |= KN_FLAG_TH; - if (Servo_data[AUX3] > PPM_SWITCH) - flags |= KN_FLAG_IDLEUP; - if (Servo_data[AUX4] > PPM_SWITCH) - flags |= KN_FLAG_GYRO3; - - packet[12] = flags; - - packet[13] = 0x00; - if(sub_protocol==WLTOYS) - packet[13] = (packet_sent << 5) | (hopping_frequency_no << 2); - - packet[14] = 0x00; - packet[15] = 0x00; - - NRF24L01_SetPower(); -} - -//------------------------------------------------------------------------------------------------- -// This function setup 24L01 -// V977 uses one way communication, receiving only. 24L01 RX is never enabled. -// V977 needs payload length in the packet. We should configure 24L01 to enable Packet Control Field(PCF) -// Some RX reg settings are actually for enable PCF -//------------------------------------------------------------------------------------------------- -static void kn_init() -{ - kn_calculate_tx_addr(); - kn_calculate_freqency_hopping_channels(); - - NRF24L01_Initialize(); - - NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO)); - NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknoledgement - NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0 - NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address - NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0); // Disable retransmit - NRF24L01_SetPower(); - NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit - NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 0x20); // bytes of data payload for pipe 0 - - - NRF24L01_Activate(0x73); - NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 1); // Dynamic payload for data pipe 0 - // Enable: Dynamic Payload Length to enable PCF - NRF24L01_WriteReg(NRF24L01_1D_FEATURE, BV(NRF2401_1D_EN_DPL)); - - NRF24L01_SetPower(); - - NRF24L01_FlushTx(); - // Turn radio power on - NRF24L01_SetTxRxMode(TX_EN); - NRF24L01_SetBitrate(NRF24L01_BR_1M); //USE1MBPS_YES ? NRF24L01_BR_1M : NRF24L01_BR_250K; -} - -//------------------------------------------------------------------------------------------------- -// This function generate RF TX packet address -// V977 can remember the binding parameters; we do not need rebind when power up. -// This requires the address must be repeatable for a specific RF ID at power up. -//------------------------------------------------------------------------------------------------- -static void kn_calculate_tx_addr() -{ - if(sub_protocol==FEILUN) - { - uint8_t addr2; - // Generate TXID with sum of minimum 256 and maximum 256+MAX_RF_CHANNEL-32 - rx_tx_addr[1] = 1 + rx_tx_addr[0] % (KN_MAX_RF_CHANNEL-33); - addr2 = 1 + rx_tx_addr[2] % (KN_MAX_RF_CHANNEL-33); - if ((uint16_t)(rx_tx_addr[0] + rx_tx_addr[1]) < 256) - rx_tx_addr[2] = addr2; - else - rx_tx_addr[2] = 0x00; - rx_tx_addr[3] = 0x00; - while((uint16_t)(rx_tx_addr[0] + rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3]) < 257) - rx_tx_addr[3] += addr2; - } - //The 5th byte is a constant, must be 'K' - rx_tx_addr[4] = 'K'; -} - -//------------------------------------------------------------------------------------------------- -// This function generates "random" RF hopping frequency channel numbers. -// These numbers must be repeatable for a specific seed -// The generated number range is from 0 to MAX_RF_CHANNEL. No repeat or adjacent numbers -// -// For Feilun variant, the channels are calculated from TXID, and since only 2 channels are used -// we copy them to fill up to MAX_RF_CHANNEL -//------------------------------------------------------------------------------------------------- -static void kn_calculate_freqency_hopping_channels() -{ - if(sub_protocol==WLTOYS) - { - uint8_t idx = 0; - uint32_t rnd = MProtocol_id; - while (idx < KN_RF_CH_COUNT) - { - uint8_t i; - rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization - - // Use least-significant byte. 73 is prime, so channels 76..77 are unused - uint8_t next_ch = ((rnd >> 8) % KN_MAX_RF_CHANNEL) + 2; - // Keep the distance 2 between the channels - either odd or even - if (((next_ch ^ MProtocol_id) & 0x01 )== 0) - continue; - // Check that it's not duplicate and spread uniformly - for (i = 0; i < idx; i++) - if(hopping_frequency[i] == next_ch) - break; - if (i != idx) - continue; - hopping_frequency[idx++] = next_ch; - } - } - else - {//FEILUN - hopping_frequency[0] = rx_tx_addr[0] + rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3]; // - 256; ??? - hopping_frequency[1] = hopping_frequency[0] + 32; - hopping_frequency[2] = hopping_frequency[0]; - hopping_frequency[3] = hopping_frequency[1]; - } -} - #endif diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index d299734..0e32599 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -89,9 +89,9 @@ uint8_t cur_protocol[2]; uint8_t prev_protocol=0; // Telemetry -#if defined(TELEMETRY) #define MAX_PKT 27 uint8_t pkt[MAX_PKT];//telemetry receiving packets +#if defined(TELEMETRY) uint8_t pktt[MAX_PKT];//telemetry receiving packets volatile uint8_t tx_head; volatile uint8_t tx_tail; @@ -176,6 +176,9 @@ void setup() //Configure PPM interrupt EICRA |=(1<> 2) + 0x20) -static void SYMAX_build_packet_x5c(uint8_t bind) +static void __attribute__((unused)) SYMAX_build_packet_x5c(uint8_t bind) { if (bind) { @@ -116,7 +116,7 @@ static void SYMAX_build_packet_x5c(uint8_t bind) } } -static void SYMAX_build_packet(uint8_t bind) +static void __attribute__((unused)) SYMAX_build_packet(uint8_t bind) { if (bind) { @@ -146,7 +146,7 @@ static void SYMAX_build_packet(uint8_t bind) packet[9] = SYMAX_checksum(packet); } -static void SYMAX_send_packet(uint8_t bind) +static void __attribute__((unused)) SYMAX_send_packet(uint8_t bind) { if (sub_protocol==SYMAX5C) SYMAX_build_packet_x5c(bind); @@ -167,7 +167,7 @@ static void SYMAX_send_packet(uint8_t bind) NRF24L01_SetPower(); // Set tx_power } -static void symax_init() +static void __attribute__((unused)) symax_init() { NRF24L01_Initialize(); // @@ -219,7 +219,7 @@ static void symax_init() NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0e); // power on } -static void symax_init1() +static void __attribute__((unused)) symax_init1() { // duplicate stock tx sending strange packet (effect unknown) uint8_t first_packet[] = {0xf9, 0x96, 0x82, 0x1b, 0x20, 0x08, 0x08, 0xf2, 0x7d, 0xef, 0xff, 0x00, 0x00, 0x00, 0x00}; @@ -247,7 +247,7 @@ static void symax_init1() } // channels determined by last byte of tx address -static void symax_set_channels(uint8_t address) +static void __attribute__((unused)) symax_set_channels(uint8_t address) { static const uint8_t start_chans_1[] = {0x0a, 0x1a, 0x2a, 0x3a}; static const uint8_t start_chans_2[] = {0x2a, 0x0a, 0x42, 0x22}; @@ -290,7 +290,7 @@ static void symax_set_channels(uint8_t address) *pchans = 0x39194121; } -static void symax_init2() +static void __attribute__((unused)) symax_init2() { uint8_t chans_data_x5c[] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22}; diff --git a/Multiprotocol/V2X2_nrf24l01.ino b/Multiprotocol/V2X2_nrf24l01.ino index 35796b1..d761c57 100644 --- a/Multiprotocol/V2X2_nrf24l01.ino +++ b/Multiprotocol/V2X2_nrf24l01.ino @@ -75,7 +75,7 @@ static const uint8_t freq_hopping[][16] = { 0x18, 0x2A, 0x21, 0x38, 0x10, 0x26, 0x20, 0x1F } // 03 }; -static void v202_init() +static void __attribute__((unused)) v202_init() { NRF24L01_Initialize(); @@ -108,7 +108,7 @@ static void v202_init() NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t *)"\x66\x88\x68\x68\x68", 5); } -static void V202_init2() +static void __attribute__((unused)) V202_init2() { NRF24L01_FlushTx(); packet_sent = 0; @@ -119,7 +119,7 @@ static void V202_init2() //Done by TX_EN??? => NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO) | BV(NRF24L01_00_PWR_UP)); } -static void V2X2_set_tx_id(void) +static void __attribute__((unused)) V2X2_set_tx_id(void) { uint8_t sum; sum = rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3]; @@ -134,7 +134,7 @@ static void V2X2_set_tx_id(void) } } -static void V2X2_add_pkt_checksum() +static void __attribute__((unused)) V2X2_add_pkt_checksum() { uint8_t sum = 0; for (uint8_t i = 0; i < 15; ++i) @@ -142,7 +142,7 @@ static void V2X2_add_pkt_checksum() packet[15] = sum; } -static void V2X2_send_packet(uint8_t bind) +static void __attribute__((unused)) V2X2_send_packet(uint8_t bind) { uint8_t flags2=0; if (bind) diff --git a/Multiprotocol/YD717_nrf24l01.ino b/Multiprotocol/YD717_nrf24l01.ino index d2a3f1a..1d9ffbe 100644 --- a/Multiprotocol/YD717_nrf24l01.ino +++ b/Multiprotocol/YD717_nrf24l01.ino @@ -41,7 +41,7 @@ enum { YD717_DATA }; -static void yd717_send_packet(uint8_t bind) +static void __attribute__((unused)) yd717_send_packet(uint8_t bind) { uint8_t rudder_trim, elevator_trim, aileron_trim; if (bind) @@ -125,7 +125,7 @@ static void yd717_send_packet(uint8_t bind) NRF24L01_SetPower(); // Set tx_power } -static void yd717_init() +static void __attribute__((unused)) yd717_init() { NRF24L01_Initialize(); @@ -162,7 +162,7 @@ static void yd717_init() NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5); } -static void YD717_init1() +static void __attribute__((unused)) YD717_init1() { // for bind packets set address to prearranged value known to receiver uint8_t bind_rx_tx_addr[] = {0x65, 0x65, 0x65, 0x65, 0x65}; @@ -179,7 +179,7 @@ static void YD717_init1() NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, bind_rx_tx_addr, 5); } -static void YD717_init2() +static void __attribute__((unused)) YD717_init2() { // set rx/tx address for data phase NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5); diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index 7eaf8d4..9a8a8d2 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -23,6 +23,7 @@ //Uncomment to enable telemetry #define TELEMETRY +#define HUB_TELEMETRY //Comment a protocol to exclude it from compilation #define BAYANG_NRF24L01_INO diff --git a/Multiprotocol/telemetry.h b/Multiprotocol/telemetry.h index b305cd7..4af2b7c 100644 --- a/Multiprotocol/telemetry.h +++ b/Multiprotocol/telemetry.h @@ -1,63 +1,149 @@ -/* - This project is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. +//************************************* +// FrSky Telemetry serial code * +// By Midelic on RCG * +//************************************* - Multiprotocol is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. +#if defined TELEMETRY - You should have received a copy of the GNU General Public License - along with Multiprotocol. If not, see . - */ +#define USER_MAX_BYTES 6 +#define MAX_PKTX 10 +uint8_t frame[18]; +uint8_t pass = 0; +uint8_t index; +uint8_t prev_index; +uint8_t pktx[MAX_PKTX]; - -static void frskySendStuffed(uint8_t frame[]) +void frskySendStuffed() { Serial_write(0x7E); - for (uint8_t i = 0; i < 9; i++) { - if ((frame[i] == 0x7e) || (frame[i] == 0x7d)) { + for (uint8_t i = 0; i < 9; i++) + { + + if ((frame[i] == 0x7e) || (frame[i] == 0x7d)) + { Serial_write(0x7D); - frame[i] ^= 0x20; + frame[i] ^= 0x20; } Serial_write(frame[i]); } Serial_write(0x7E); } -static void frskySendFrame() -{ - uint8_t frame[9]; +void compute_RSSIdbm(){ + RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>5); + if(pktt[len-2] >=128) + RSSI_dBm -= 82; + else + RSSI_dBm += 65; +} - frame[0] = 0xfe; - if ((cur_protocol[0]&0x1F)==MODE_FRSKY) - { - compute_RSSIdbm(); - frame[1] = pktt[3]; - frame[2] = pktt[4]; - frame[3] = (uint8_t)RSSI_dBm; - frame[4] = pktt[5]*2;//txrssi +void frsky_link_frame() +{ + frame[0] = 0xFE; + if ((cur_protocol[0]&0x1F)==MODE_FRSKY) + { + compute_RSSIdbm(); + frame[1] = pktt[3]; + frame[2] = pktt[4]; + frame[3] = (uint8_t)RSSI_dBm; + frame[4] = pktt[5]*2; + frame[5] = frame[6] = frame[7] = frame[8] = 0; + } + else + if ((cur_protocol[0]&0x1F)==MODE_HUBSAN) + { + frame[1] = v_lipo*2; //v_lipo; common 0x2A=42/10=4.2V + frame[2] = frame[1]; + frame[3] =0X6e; + frame[4] =2*0x6e; frame[5] = frame[6] = frame[7] = frame[8] = 0; } - else - if ((cur_protocol[0]&0x1F)==MODE_HUBSAN) - { - frame[1] = v_lipo*2; - frame[2] = 0; - frame[3] = 0x5A;//dummy value - frame[4] = 2 * 0x5A;//dummy value - frame[5] = frame[6] = frame[7] = frame[8] = 0; - } - frskySendStuffed(frame); + frskySendStuffed(); } +#if defined HUB_TELEMETRY +void frsky_user_frame() +{ + uint8_t indexx = 0, c=0, j=8, n=0, i; + + if(pktt[6]>0 && pktt[6]<=MAX_PKTX) + {//only valid hub frames + frame[0] = 0xFD; + frame[1] = 0; + frame[2] = pktt[7]; + + switch(pass) + { + case 0: + indexx=pktt[6]; + for(i=0;i