mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2025-02-04 16:48:10 +00:00
Hub telemetry and fix compilation warnings/errors if protocols are commented
This commit is contained in:
pascallanger
parent
a689ce4de9
commit
b50bedef39
@ -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;
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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));
|
||||
|
||||
@ -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];
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -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;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()
|
||||
{
|
||||
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;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
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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<<ISC11); // The rising edge of INT1 pin D3 generates an interrupt request
|
||||
EIMSK |= (1<<INT1); // INT1 interrupt enable
|
||||
#if defined(TELEMETRY)
|
||||
PPM_Telemetry_serial_init(); // Configure serial for telemetry
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{ // Serial
|
||||
@ -580,6 +583,16 @@ static void Mprotocol_serial_init()
|
||||
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
|
||||
static void set_rx_tx_addr(uint32_t id)
|
||||
{ // Used by almost all protocols
|
||||
|
||||
@ -31,7 +31,7 @@ enum {
|
||||
SLT_DATA3
|
||||
};
|
||||
|
||||
static void SLT_init()
|
||||
static void __attribute__((unused)) SLT_init()
|
||||
{
|
||||
NRF24L01_Initialize();
|
||||
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();
|
||||
}
|
||||
|
||||
static void SLT_init2()
|
||||
static void __attribute__((unused)) SLT_init2()
|
||||
{
|
||||
NRF24L01_FlushTx();
|
||||
packet_sent = 0;
|
||||
@ -57,7 +57,7 @@ static void SLT_init2()
|
||||
NRF24L01_SetTxRxMode(TX_EN);
|
||||
}
|
||||
|
||||
static void SLT_set_tx_id(void)
|
||||
static void __attribute__((unused)) SLT_set_tx_id(void)
|
||||
{
|
||||
// Frequency hopping sequence generation
|
||||
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);
|
||||
}
|
||||
|
||||
static void SLT_wait_radio()
|
||||
static void __attribute__((unused)) SLT_wait_radio()
|
||||
{
|
||||
if (packet_sent)
|
||||
while (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_TX_DS))) ;
|
||||
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();
|
||||
NRF24L01_FlushTx();
|
||||
@ -107,7 +107,7 @@ static void SLT_send_data(uint8_t *data, uint8_t len)
|
||||
packet_sent = 1;
|
||||
}
|
||||
|
||||
static void SLT_build_packet()
|
||||
static void __attribute__((unused)) SLT_build_packet()
|
||||
{
|
||||
// aileron, elevator, throttle, rudder, gear, pitch
|
||||
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;
|
||||
}
|
||||
|
||||
static void SLT_send_bind_packet()
|
||||
static void __attribute__((unused)) SLT_send_bind_packet()
|
||||
{
|
||||
SLT_wait_radio();
|
||||
BIND_IN_PROGRESS; // autobind protocol
|
||||
|
||||
@ -41,7 +41,7 @@ enum {
|
||||
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];
|
||||
|
||||
@ -54,7 +54,7 @@ static uint8_t SYMAX_checksum(uint8_t *data)
|
||||
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
|
||||
// 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)
|
||||
|
||||
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};
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#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<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()
|
||||
{
|
||||
if(telemetry_link)
|
||||
{
|
||||
frskySendFrame();
|
||||
frsky_link_frame();
|
||||
telemetry_link=0;
|
||||
return;
|
||||
}
|
||||
#if defined HUB_TELEMETRY
|
||||
if(!telemetry_link)
|
||||
frsky_user_frame();
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
Loading…
x
Reference in New Issue
Block a user