gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-04 19:48:11 +00:00
Code Issues Projects Releases Wiki Activity

Hub telemetry and fix compilation warnings/errors if protocols are commented

Browse Source
This commit is contained in:
pascallanger 2016-01-27 17:57:33 +01:00
parent a689ce4de9
commit b50bedef39
18 changed files with 578 additions and 483 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Multiprotocol/Bayang_nrf24l01.ino
View File

@ -38,7 +38,7 @@ enum BAYANG_FLAGS {
BAYANG_FLAG_INVERTED = 0x80 // inverted flight on Floureon H101 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; uint8_t i;
if (bind) if (bind)
@ -112,7 +112,7 @@ static void BAYANG_send_packet(uint8_t bind)
NRF24L01_SetPower(); // Set tx_power NRF24L01_SetPower(); // Set tx_power
} }
static void BAYANG_init() static void __attribute__((unused)) BAYANG_init()
{ {
NRF24L01_Initialize(); NRF24L01_Initialize();
NRF24L01_SetTxRxMode(TX_EN); NRF24L01_SetTxRxMode(TX_EN);
@ -148,7 +148,7 @@ uint16_t BAYANG_callback()
return BAYANG_PACKET_PERIOD; 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... //Could be using txid[0..2] but using rx_tx_addr everywhere instead...
hopping_frequency[0]=0; hopping_frequency[0]=0;

6
Multiprotocol/CG023_nrf24l01.ino
View File

@ -70,7 +70,7 @@ enum H8_3D_FLAGS_2 {
H8_3D_FLAG_CALIBRATE = 0x20, // accelerometer calibration 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 : 0x00 - 0xFF
throttle=convert_channel_8b(THROTTLE); throttle=convert_channel_8b(THROTTLE);
@ -205,7 +205,7 @@ static void CG023_send_packet(uint8_t bind)
NRF24L01_SetPower(); // Set tx_power NRF24L01_SetPower(); // Set tx_power
} }
static void CG023_init() static void __attribute__((unused)) CG023_init()
{ {
NRF24L01_Initialize(); NRF24L01_Initialize();
NRF24L01_SetTxRxMode(TX_EN); NRF24L01_SetTxRxMode(TX_EN);
@ -246,7 +246,7 @@ uint16_t CG023_callback()
return H8_3D_PACKET_PERIOD; return H8_3D_PACKET_PERIOD;
} }
static void CG023_initialize_txid() static void __attribute__((unused)) CG023_initialize_txid()
{ {
if(sub_protocol==H8_3D) if(sub_protocol==H8_3D)
{ {

6
Multiprotocol/CX10_nrf24l01.ino
View File

@ -46,7 +46,7 @@ enum {
CX10_DATA CX10_DATA
}; };
static void CX10_Write_Packet(uint8_t bind) static void __attribute__((unused)) CX10_Write_Packet(uint8_t bind)
{ {
uint8_t offset = 0; uint8_t offset = 0;
if(sub_protocol == CX10_BLUE) if(sub_protocol == CX10_BLUE)
@ -166,7 +166,7 @@ static void CX10_Write_Packet(uint8_t bind)
NRF24L01_SetPower(); NRF24L01_SetPower();
} }
static void CX10_init() static void __attribute__((unused)) CX10_init()
{ {
NRF24L01_Initialize(); NRF24L01_Initialize();
NRF24L01_SetTxRxMode(TX_EN); NRF24L01_SetTxRxMode(TX_EN);
@ -226,7 +226,7 @@ uint16_t CX10_callback() {
return packet_period; return packet_period;
} }
static void initialize_txid() static void __attribute__((unused)) initialize_txid()
{ {
rx_tx_addr[1]%= 0x30; rx_tx_addr[1]%= 0x30;
if(sub_protocol==Q282) if(sub_protocol==Q282)

48
Multiprotocol/DSM2_cyrf6936.ino
View File

@ -120,7 +120,7 @@ const uint8_t cyrfmfg_id[6] = {0xd4, 0x62, 0xd6, 0xad, 0xd3, 0xff}; //dx6i
#endif #endif
*/ */
static void build_bind_packet() static void __attribute__((unused)) build_bind_packet()
{ {
uint8_t i; uint8_t i;
uint16_t sum = 384 - 0x10;// uint16_t sum = 384 - 0x10;//
@ -153,7 +153,23 @@ static void build_bind_packet()
packet[15] = sum & 0xff; 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 #if DSM2_NUM_CHANNELS==4
const uint8_t ch_map[] = {0, 1, 2, 3, 0xff, 0xff, 0xff}; //Guess 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) static uint8_t __attribute__((unused)) get_pn_row(uint8_t channel)
{
#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)
{ {
return (sub_protocol == DSMX ? (channel - 2) % 5 : channel % 5); 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 {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++) for(uint8_t i = 0; i < sizeof(init_vals) / 2; i++)
CYRF_WriteRegister(init_vals[i][0], init_vals[i][1]); CYRF_WriteRegister(init_vals[i][0], init_vals[i][1]);
@ -307,7 +307,7 @@ static void cyrf_config()
CYRF_ConfigRFChannel(0x61); 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 }; const uint8_t pn_bind[] = { 0xc6,0x94,0x22,0xfe,0x48,0xe6,0x57,0x4e };
uint8_t data_code[32]; uint8_t data_code[32];
@ -341,13 +341,13 @@ const uint8_t data_vals[][2] = {
{CYRF_10_FRAMING_CFG, 0xea}, {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++) for(uint8_t i = 0; i < sizeof(data_vals) / 2; i++)
CYRF_WriteRegister(data_vals[i][0], data_vals[i][1]); 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]); 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); //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; crcidx = !crcidx;
} }
static void calc_dsmx_channel() static void __attribute__((unused)) calc_dsmx_channel()
{ {
uint8_t idx = 0; 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)); 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));

22
Multiprotocol/Devo_cyrf6936.ino
View File

@ -66,7 +66,7 @@ uint8_t ch_idx;
uint8_t use_fixed_id; uint8_t use_fixed_id;
uint8_t failsafe_pkt; uint8_t failsafe_pkt;
static void scramble_pkt() static void __attribute__((unused)) scramble_pkt()
{ {
#ifdef NO_SCRAMBLE #ifdef NO_SCRAMBLE
return; return;
@ -77,7 +77,7 @@ static void scramble_pkt()
#endif #endif
} }
static void add_pkt_suffix() static void __attribute__((unused)) add_pkt_suffix()
{ {
uint8_t bind_state; uint8_t bind_state;
if (use_fixed_id) if (use_fixed_id)
@ -97,7 +97,7 @@ static void add_pkt_suffix()
packet[15] = (fixed_id >> 16) & 0xff; 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); packet[0] = ((DEVO_NUM_CHANNELS << 4) | 0x07);
// uint8_t enable = 0; // uint8_t enable = 0;
@ -116,7 +116,7 @@ static void build_beacon_pkt(uint8_t upper)
add_pkt_suffix(); add_pkt_suffix();
} }
static void build_bind_pkt() static void __attribute__((unused)) build_bind_pkt()
{ {
packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x0a; packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x0a;
packet[1] = bind_counter & 0xff; packet[1] = bind_counter & 0xff;
@ -136,7 +136,7 @@ static void build_bind_pkt()
packet[15] ^= cyrfmfg_id[2]; packet[15] ^= cyrfmfg_id[2];
} }
static void build_data_pkt() static void __attribute__((unused)) build_data_pkt()
{ {
uint8_t i; uint8_t i;
packet[0] = (DEVO_NUM_CHANNELS << 4) | (0x0b + ch_idx); packet[0] = (DEVO_NUM_CHANNELS << 4) | (0x0b + ch_idx);
@ -161,7 +161,7 @@ static void build_data_pkt()
add_pkt_suffix(); 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 */ /* 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]); 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); CYRF_SetPower(0x08);
} }
static void cyrf_init() static void __attribute__((unused)) cyrf_init()
{ {
/* Initialise CYRF chip */ /* Initialise CYRF chip */
CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x39); CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x39);
@ -201,7 +201,7 @@ static void cyrf_init()
CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28); CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28);
} }
static void set_radio_channels() static void __attribute__((unused)) set_radio_channels()
{ {
//int i; //int i;
CYRF_FindBestChannels(hopping_frequency, 3, 4, 4, 80); CYRF_FindBestChannels(hopping_frequency, 3, 4, 4, 80);
@ -217,7 +217,7 @@ static void set_radio_channels()
hopping_frequency[4] = hopping_frequency[1]; hopping_frequency[4] = hopping_frequency[1];
} }
static void DEVO_BuildPacket() static void __attribute__((unused)) DEVO_BuildPacket()
{ {
switch(phase) switch(phase)
{ {
@ -302,7 +302,7 @@ uint16_t devo_callback()
return 1200; return 1200;
} }
/*static void devo_bind() /*static void __attribute__((unused)) devo_bind()
{ {
fixed_id = Model_fixed_id; fixed_id = Model_fixed_id;
bind_counter = DEVO_BIND_COUNT; 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){ if(BIND_0){
//randomSeed((uint32_t)analogRead(A6)<<10|analogRead(A7));//seed //randomSeed((uint32_t)analogRead(A6)<<10|analogRead(A7));//seed
uint8_t txid[4]; uint8_t txid[4];

8
Multiprotocol/ESky_nrf24l01.ino
View File

@ -23,14 +23,14 @@
#define ESKY_PAYLOAD_SIZE 13 #define ESKY_PAYLOAD_SIZE 13
#define ESKY_PACKET_CHKTIME 100 // Time to wait for packet to be sent (no ACK, so very short) #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_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_0A_RX_ADDR_P0, rx_tx_addr, 4);
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, 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(); 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 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(); NRF24L01_FlushTx();
packet_sent = 0; packet_sent = 0;
@ -90,7 +90,7 @@ static void ESKY_init2()
NRF24L01_SetTxRxMode(TX_EN); 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 uint8_t rf_ch = 50; // bind channel
if (bind) if (bind)

4
Multiprotocol/FlySky_a7105.ino
View File

@ -73,7 +73,7 @@ uint8_t chanrow;
uint8_t chancol; uint8_t chancol;
uint8_t chanoffset; 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 ? const uint8_t V912_X17_SEQ[10] = { 0x14, 0x31, 0x40, 0x49, 0x49, // sometime first byte is 0x15 ?
0x49, 0x49, 0x49, 0x49, 0x49, }; 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; uint8_t i;
//servodata timing range for flysky. //servodata timing range for flysky.

308
Multiprotocol/FrSky_cc2500.ino
View File

@ -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() uint16_t initFrSky_2way()
{ {
if(IS_AUTOBIND_FLAG_on) if(IS_AUTOBIND_FLAG_on)
@ -49,6 +178,26 @@ uint16_t initFrSky_2way()
return 10000; 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;i<len;i++)
pktt[i]=pkt[i];
telemetry_link=1;
if(pktt[6]>0)
telemetry_counter=(telemetry_counter+1)%32;
}
}
#endif
uint16_t ReadFrSky_2way() uint16_t ReadFrSky_2way()
{ {
if (state < FRSKY_BIND_DONE) if (state < FRSKY_BIND_DONE)
@ -112,163 +261,4 @@ uint16_t ReadFrSky_2way()
} }
return state == FRSKY_DATA4 ? 7500 : 9000; 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;i<len;i++)
pktt[i]=pkt[i];
telemetry_link=1;
if(pktt[6]>0)
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 #endif

10
Multiprotocol/Hisky_nrf24l01.ino
View File

@ -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 // 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 // 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] // 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; uint8_t idx = 0;
uint32_t rnd = MProtocol_id; 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; uint8_t i;
uint16_t sum=0; 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(); 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 // 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 // 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; uint16_t temp;
uint8_t i,j; uint8_t i,j;
@ -217,7 +217,7 @@ uint16_t hisky_cb()
return 1000; // send 1 binding packet and 1 data packet per 9ms 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 //Generate frequency hopping table
if(sub_protocol==HK310) if(sub_protocol==HK310)

16
Multiprotocol/Hubsan_a7105.ino
View File

@ -56,7 +56,7 @@ enum {
}; };
#define WAIT_WRITE 0x80 #define WAIT_WRITE 0x80
static void update_crc() static void __attribute__((unused)) hubsan_update_crc()
{ {
uint8_t sum = 0; uint8_t sum = 0;
for(uint8_t i = 0; i < 15; i++) for(uint8_t i = 0; i < 15; i++)
@ -64,7 +64,7 @@ static void update_crc()
packet[15] = (256 - (sum % 256)) & 0xFF; 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; static uint8_t handshake_counter;
if(phase < BIND_7) if(phase < BIND_7)
@ -99,14 +99,14 @@ static void hubsan_build_bind_packet(uint8_t bind_state)
if(phase == BIND_7) if(phase == BIND_7)
packet[2] = handshake_counter++; packet[2] = handshake_counter++;
} }
update_crc(); hubsan_update_crc();
} }
//cc : throttle observed range: 0x00 - 0xFF (smaller is down) //cc : throttle observed range: 0x00 - 0xFF (smaller is down)
//ee : rudder observed range: 0x34 - 0xcc (smaller is right)52-204-60% //ee : rudder observed range: 0x34 - 0xcc (smaller is right)52-204-60%
//gg : elevator observed range: 0x3e - 0xbc (smaller is up)62-188 -50% //gg : elevator observed range: 0x3e - 0xbc (smaller is up)62-188 -50%
//ii : aileron observed range: 0x45 - 0xc3 (smaller is right)69-195-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; static uint8_t vtx_freq = 0;
memset(packet, 0, 16); memset(packet, 0, 16);
@ -177,7 +177,7 @@ static void hubsan_build_packet()
packet_count++; packet_count++;
} }
} }
update_crc(); hubsan_update_crc();
} }
#if defined(TELEMETRY) #if defined(TELEMETRY)
@ -193,8 +193,10 @@ static void hubsan_build_packet()
uint16_t ReadHubsan() uint16_t ReadHubsan()
{ {
static uint8_t txState=0; #if defined(TELEMETRY)
static uint8_t rfMode=0; static uint8_t rfMode=0;
#endif
static uint8_t txState=0;
static uint8_t bind_count=0; static uint8_t bind_count=0;
uint16_t delay; uint16_t delay;
uint8_t i; uint8_t i;
@ -277,7 +279,9 @@ uint16_t ReadHubsan()
case DATA_4: case DATA_4:
case DATA_5: case DATA_5:
if( txState == 0) { // send packet if( txState == 0) { // send packet
#if defined(TELEMETRY)
rfMode = A7105_TX; rfMode = A7105_TX;
#endif
if( phase == DATA_1) if( phase == DATA_1)
A7105_SetPower(); //Keep transmit power in sync A7105_SetPower(); //Keep transmit power in sync
hubsan_build_packet(); hubsan_build_packet();

397
Multiprotocol/KN_nrf24l01.ino
View File

@ -70,6 +70,205 @@ enum {
KN_FLAG_GYROR = 0x80 // Always 0 so far 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 // Private Functions
//================================================================================================ //================================================================================================
@ -145,202 +344,4 @@ uint16_t kn_callback()
return packet_period; 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 #endif

15
Multiprotocol/Multiprotocol.ino
View File

@ -89,9 +89,9 @@ uint8_t cur_protocol[2];
uint8_t prev_protocol=0; uint8_t prev_protocol=0;
// Telemetry // Telemetry
#if defined(TELEMETRY)
#define MAX_PKT 27 #define MAX_PKT 27
uint8_t pkt[MAX_PKT];//telemetry receiving packets uint8_t pkt[MAX_PKT];//telemetry receiving packets
#if defined(TELEMETRY)
uint8_t pktt[MAX_PKT];//telemetry receiving packets uint8_t pktt[MAX_PKT];//telemetry receiving packets
volatile uint8_t tx_head; volatile uint8_t tx_head;
volatile uint8_t tx_tail; volatile uint8_t tx_tail;
@ -176,6 +176,9 @@ void setup()
//Configure PPM interrupt //Configure PPM interrupt
EICRA |=(1<<ISC11); // The rising edge of INT1 pin D3 generates an interrupt request EICRA |=(1<<ISC11); // The rising edge of INT1 pin D3 generates an interrupt request
EIMSK |= (1<<INT1); // INT1 interrupt enable EIMSK |= (1<<INT1); // INT1 interrupt enable
#if defined(TELEMETRY)
PPM_Telemetry_serial_init(); // Configure serial for telemetry
#endif
} }
else else
{ // Serial { // Serial
@ -580,6 +583,16 @@ static void Mprotocol_serial_init()
UCSR0B |= (1<<TXEN0);//tx enable UCSR0B |= (1<<TXEN0);//tx enable
} }
static void PPM_Telemetry_serial_init()
{
//9600 bauds
UBRR0H = 0x00;
UBRR0L = 0x67;
//Set frame format to 8 data bits, none, 1 stop bit
UCSR0C |= (1<<UCSZ01)|(1<<UCSZ00);
UCSR0B |= (1<<TXEN0);//tx enable
}
// Convert 32b id to rx_tx_addr // Convert 32b id to rx_tx_addr
static void set_rx_tx_addr(uint32_t id) static void set_rx_tx_addr(uint32_t id)
{ // Used by almost all protocols { // Used by almost all protocols

14
Multiprotocol/SLT_nrf24l01.ino
View File

@ -31,7 +31,7 @@ enum {
SLT_DATA3 SLT_DATA3
}; };
static void SLT_init() static void __attribute__((unused)) SLT_init()
{ {
NRF24L01_Initialize(); NRF24L01_Initialize();
NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO)); // 2-bytes CRC, radio off NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO)); // 2-bytes CRC, radio off
@ -47,7 +47,7 @@ static void SLT_init()
NRF24L01_FlushRx(); NRF24L01_FlushRx();
} }
static void SLT_init2() static void __attribute__((unused)) SLT_init2()
{ {
NRF24L01_FlushTx(); NRF24L01_FlushTx();
packet_sent = 0; packet_sent = 0;
@ -57,7 +57,7 @@ static void SLT_init2()
NRF24L01_SetTxRxMode(TX_EN); NRF24L01_SetTxRxMode(TX_EN);
} }
static void SLT_set_tx_id(void) static void __attribute__((unused)) SLT_set_tx_id(void)
{ {
// Frequency hopping sequence generation // Frequency hopping sequence generation
for (uint8_t i = 0; i < 4; ++i) for (uint8_t i = 0; i < 4; ++i)
@ -90,14 +90,14 @@ static void SLT_set_tx_id(void)
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 4); NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 4);
} }
static void SLT_wait_radio() static void __attribute__((unused)) SLT_wait_radio()
{ {
if (packet_sent) if (packet_sent)
while (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_TX_DS))) ; while (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_TX_DS))) ;
packet_sent = 0; packet_sent = 0;
} }
static void SLT_send_data(uint8_t *data, uint8_t len) static void __attribute__((unused)) SLT_send_data(uint8_t *data, uint8_t len)
{ {
SLT_wait_radio(); SLT_wait_radio();
NRF24L01_FlushTx(); NRF24L01_FlushTx();
@ -107,7 +107,7 @@ static void SLT_send_data(uint8_t *data, uint8_t len)
packet_sent = 1; packet_sent = 1;
} }
static void SLT_build_packet() static void __attribute__((unused)) SLT_build_packet()
{ {
// aileron, elevator, throttle, rudder, gear, pitch // aileron, elevator, throttle, rudder, gear, pitch
uint8_t e = 0; // byte where extension 2 bits for every 10-bit channel are packed uint8_t e = 0; // byte where extension 2 bits for every 10-bit channel are packed
@ -129,7 +129,7 @@ static void SLT_build_packet()
hopping_frequency_no = 0; hopping_frequency_no = 0;
} }
static void SLT_send_bind_packet() static void __attribute__((unused)) SLT_send_bind_packet()
{ {
SLT_wait_radio(); SLT_wait_radio();
BIND_IN_PROGRESS; // autobind protocol BIND_IN_PROGRESS; // autobind protocol

18
Multiprotocol/Symax_nrf24l01.ino
View File

@ -41,7 +41,7 @@ enum {
SYMAX_DATA SYMAX_DATA
}; };
static uint8_t SYMAX_checksum(uint8_t *data) static uint8_t __attribute__((unused)) SYMAX_checksum(uint8_t *data)
{ {
uint8_t sum = data[0]; uint8_t sum = data[0];
@ -54,7 +54,7 @@ static uint8_t SYMAX_checksum(uint8_t *data)
return sum + ( sub_protocol==SYMAX5C ? 0 : 0x55 ); return sum + ( sub_protocol==SYMAX5C ? 0 : 0x55 );
} }
static void SYMAX_read_controls() static void __attribute__((unused)) SYMAX_read_controls()
{ {
// Protocol is registered AETRF, that is // Protocol is registered AETRF, that is
// Aileron is channel 1, Elevator - 2, Throttle - 3, Rudder - 4, Flip control - 5 // Aileron is channel 1, Elevator - 2, Throttle - 3, Rudder - 4, Flip control - 5
@ -80,7 +80,7 @@ static void SYMAX_read_controls()
#define X5C_CHAN2TRIM(X) ((((X) & 0x80 ? 0xff - (X) : 0x80 + (X)) >> 2) + 0x20) #define X5C_CHAN2TRIM(X) ((((X) & 0x80 ? 0xff - (X) : 0x80 + (X)) >> 2) + 0x20)
static void SYMAX_build_packet_x5c(uint8_t bind) static void __attribute__((unused)) SYMAX_build_packet_x5c(uint8_t bind)
{ {
if (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) if (bind)
{ {
@ -146,7 +146,7 @@ static void SYMAX_build_packet(uint8_t bind)
packet[9] = SYMAX_checksum(packet); 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) if (sub_protocol==SYMAX5C)
SYMAX_build_packet_x5c(bind); SYMAX_build_packet_x5c(bind);
@ -167,7 +167,7 @@ static void SYMAX_send_packet(uint8_t bind)
NRF24L01_SetPower(); // Set tx_power NRF24L01_SetPower(); // Set tx_power
} }
static void symax_init() static void __attribute__((unused)) symax_init()
{ {
NRF24L01_Initialize(); NRF24L01_Initialize();
// //
@ -219,7 +219,7 @@ static void symax_init()
NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0e); // power on 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) // 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}; 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 // 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_1[] = {0x0a, 0x1a, 0x2a, 0x3a};
static const uint8_t start_chans_2[] = {0x2a, 0x0a, 0x42, 0x22}; 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; *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, uint8_t chans_data_x5c[] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24,
0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22}; 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};

10
Multiprotocol/V2X2_nrf24l01.ino
View File

@ -75,7 +75,7 @@ static const uint8_t freq_hopping[][16] = {
0x18, 0x2A, 0x21, 0x38, 0x10, 0x26, 0x20, 0x1F } // 03 0x18, 0x2A, 0x21, 0x38, 0x10, 0x26, 0x20, 0x1F } // 03
}; };
static void v202_init() static void __attribute__((unused)) v202_init()
{ {
NRF24L01_Initialize(); NRF24L01_Initialize();
@ -108,7 +108,7 @@ static void v202_init()
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t *)"\x66\x88\x68\x68\x68", 5); 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(); NRF24L01_FlushTx();
packet_sent = 0; 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)); //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; uint8_t sum;
sum = rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3]; 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; uint8_t sum = 0;
for (uint8_t i = 0; i < 15; ++i) for (uint8_t i = 0; i < 15; ++i)
@ -142,7 +142,7 @@ static void V2X2_add_pkt_checksum()
packet[15] = sum; 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; uint8_t flags2=0;
if (bind) if (bind)

8
Multiprotocol/YD717_nrf24l01.ino
View File

@ -41,7 +41,7 @@ enum {
YD717_DATA 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; uint8_t rudder_trim, elevator_trim, aileron_trim;
if (bind) if (bind)
@ -125,7 +125,7 @@ static void yd717_send_packet(uint8_t bind)
NRF24L01_SetPower(); // Set tx_power NRF24L01_SetPower(); // Set tx_power
} }
static void yd717_init() static void __attribute__((unused)) yd717_init()
{ {
NRF24L01_Initialize(); NRF24L01_Initialize();
@ -162,7 +162,7 @@ static void yd717_init()
NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5); 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 // for bind packets set address to prearranged value known to receiver
uint8_t bind_rx_tx_addr[] = {0x65, 0x65, 0x65, 0x65, 0x65}; 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); 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 // set rx/tx address for data phase
NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5); NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5);

1
Multiprotocol/_Config.h
View File

@ -23,6 +23,7 @@
//Uncomment to enable telemetry //Uncomment to enable telemetry
#define TELEMETRY #define TELEMETRY
#define HUB_TELEMETRY
//Comment a protocol to exclude it from compilation //Comment a protocol to exclude it from compilation
#define BAYANG_NRF24L01_INO #define BAYANG_NRF24L01_INO

164
Multiprotocol/telemetry.h
View File

@ -1,63 +1,149 @@
/* //*************************************
This project is free software: you can redistribute it and/or modify // FrSky Telemetry serial code *
it under the terms of the GNU General Public License as published by // By Midelic on RCG *
the Free Software Foundation, either version 3 of the License, or //*************************************
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful, #if defined TELEMETRY
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.
You should have received a copy of the GNU General Public License #define USER_MAX_BYTES 6
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>. #define MAX_PKTX 10
*/ uint8_t frame[18];
uint8_t pass = 0;
uint8_t index;
uint8_t prev_index;
uint8_t pktx[MAX_PKTX];
void frskySendStuffed()
static void frskySendStuffed(uint8_t frame[])
{ {
Serial_write(0x7E); Serial_write(0x7E);
for (uint8_t i = 0; i < 9; i++) { for (uint8_t i = 0; i < 9; i++)
if ((frame[i] == 0x7e) || (frame[i] == 0x7d)) { {
if ((frame[i] == 0x7e) || (frame[i] == 0x7d))
{
Serial_write(0x7D); Serial_write(0x7D);
frame[i] ^= 0x20; frame[i] ^= 0x20;
} }
Serial_write(frame[i]); Serial_write(frame[i]);
} }
Serial_write(0x7E); Serial_write(0x7E);
} }
static void frskySendFrame() void compute_RSSIdbm(){
{ RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>5);
uint8_t frame[9]; if(pktt[len-2] >=128)
RSSI_dBm -= 82;
else
RSSI_dBm += 65;
}
frame[0] = 0xfe; void frsky_link_frame()
if ((cur_protocol[0]&0x1F)==MODE_FRSKY) {
{ frame[0] = 0xFE;
compute_RSSIdbm(); if ((cur_protocol[0]&0x1F)==MODE_FRSKY)
frame[1] = pktt[3]; {
frame[2] = pktt[4]; compute_RSSIdbm();
frame[3] = (uint8_t)RSSI_dBm; frame[1] = pktt[3];
frame[4] = pktt[5]*2;//txrssi 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; frame[5] = frame[6] = frame[7] = frame[8] = 0;
} }
else frskySendStuffed();
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);
} }
#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<indexx;i++)
{
if(pktt[j]==0x5E)
{
if(c++)
{
c=0;
n++;
j++;
}
}
pktx[i]=pktt[j++];
}
indexx = indexx-n;
pass=1;
case 1:
index=indexx;
prev_index = indexx;
if(index<USER_MAX_BYTES)
{
for(i=0;i<index;i++)
frame[i+3]=pktx[i];
pktt[6]=0;
pass=0;
}
else
{
index = USER_MAX_BYTES;
for(i=0;i<index;i++)
frame[i+3]=pktx[i];
pass=2;
}
break;
case 2:
index = prev_index - index;
prev_index=0;
if(index<MAX_PKTX-USER_MAX_BYTES) //10-6=4
for(i=0;i<index;i++)
frame[i+3]=pktx[USER_MAX_BYTES+i];
pass=0;
pktt[6]=0;
break;
default:
break;
}
if(!index)
return;
frame[1] = index;
frskySendStuffed();
}
else
pass=0;
}
#endif
void frskyUpdate() void frskyUpdate()
{ {
if(telemetry_link) if(telemetry_link)
{ {
frskySendFrame(); frsky_link_frame();
telemetry_link=0; telemetry_link=0;
return;
} }
#if defined HUB_TELEMETRY
if(!telemetry_link)
frsky_user_frame();
#endif
} }
#endif