You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
352 lines
9.8 KiB
352 lines
9.8 KiB
/*
|
|
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.
|
|
|
|
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.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include "cc2500_spi.h"
|
|
#include "Multiprotocol.h"
|
|
#include "input.h"
|
|
#include "FrSkyD_cc2500.h"
|
|
#include "telemetry.h"
|
|
uint8_t v_lipo1;
|
|
uint8_t v_lipo2;
|
|
uint8_t RX_RSSI;
|
|
uint8_t TX_RSSI;
|
|
uint8_t RX_LQI;
|
|
uint8_t TX_LQI;
|
|
uint8_t telemetry_link=0;
|
|
uint8_t telemetry_lost;
|
|
uint8_t telemetry_counter=0;
|
|
uint8_t rx_tx_addr[5];
|
|
uint8_t hopping_frequency[50];
|
|
uint8_t pkt[MAX_PKT];//telemetry receiving packets
|
|
uint8_t pktt[MAX_PKT];//telemetry receiving packets
|
|
|
|
uint16_t state;
|
|
uint16_t counter;
|
|
|
|
uint8_t freq_offset;
|
|
uint8_t freq_offset_prev;
|
|
|
|
const uint8_t FRSKY_common_startreg_cc2500_conf[] = {
|
|
CC2500_02_IOCFG0 ,
|
|
CC2500_00_IOCFG2 ,
|
|
CC2500_17_MCSM1 ,
|
|
CC2500_18_MCSM0 ,
|
|
CC2500_06_PKTLEN ,
|
|
CC2500_07_PKTCTRL1 ,
|
|
CC2500_08_PKTCTRL0 ,
|
|
CC2500_3E_PATABLE ,
|
|
CC2500_0B_FSCTRL1 ,
|
|
CC2500_0C_FSCTRL0 , // replaced by frequency offset value
|
|
CC2500_0D_FREQ2 ,
|
|
CC2500_0E_FREQ1 ,
|
|
CC2500_0F_FREQ0 ,
|
|
CC2500_10_MDMCFG4 ,
|
|
CC2500_11_MDMCFG3 ,
|
|
CC2500_12_MDMCFG2 ,
|
|
CC2500_13_MDMCFG1 ,
|
|
CC2500_14_MDMCFG0 ,
|
|
CC2500_15_DEVIATN
|
|
};
|
|
|
|
const uint8_t FRSKY_common_end_cc2500_conf[][2] = {
|
|
{ CC2500_19_FOCCFG, 0x16 },
|
|
{ CC2500_1A_BSCFG, 0x6c },
|
|
{ CC2500_1B_AGCCTRL2, 0x43 },
|
|
{ CC2500_1C_AGCCTRL1, 0x40 },
|
|
{ CC2500_1D_AGCCTRL0, 0x91 },
|
|
{ CC2500_21_FREND1, 0x56 },
|
|
{ CC2500_22_FREND0, 0x10 },
|
|
{ CC2500_23_FSCAL3, 0xa9 },
|
|
{ CC2500_24_FSCAL2, 0x0A },
|
|
{ CC2500_25_FSCAL1, 0x00 },
|
|
{ CC2500_26_FSCAL0, 0x11 },
|
|
{ CC2500_29_FSTEST, 0x59 },
|
|
{ CC2500_2C_TEST2, 0x88 },
|
|
{ CC2500_2D_TEST1, 0x31 },
|
|
{ CC2500_2E_TEST0, 0x0B },
|
|
{ CC2500_03_FIFOTHR, 0x07 },
|
|
{ CC2500_09_ADDR, 0x00 }
|
|
};
|
|
|
|
const uint8_t FRSKYD_cc2500_conf[] = {
|
|
/*02_IOCFG0*/ 0x06 ,
|
|
/*00_IOCFG2*/ 0x06 ,
|
|
/*17_MCSM1*/ 0x0c ,
|
|
/*18_MCSM0*/ 0x18 ,
|
|
/*06_PKTLEN*/ 0x19 ,
|
|
/*07_PKTCTRL1*/ 0x04 ,
|
|
/*08_PKTCTRL0*/ 0x05 ,
|
|
/*3E_PATABLE*/ 0xff ,
|
|
/*0B_FSCTRL1*/ 0x08 ,
|
|
/*0C_FSCTRL0*/ 0x00 ,
|
|
/*0D_FREQ2*/ 0x5c ,
|
|
/*0E_FREQ1*/ 0x76 ,
|
|
/*0F_FREQ0*/ 0x27 ,
|
|
/*10_MDMCFG4*/ 0xAA ,
|
|
/*11_MDMCFG3*/ 0x39 ,
|
|
/*12_MDMCFG2*/ 0x11 ,
|
|
/*13_MDMCFG1*/ 0x23 ,
|
|
/*14_MDMCFG0*/ 0x7a ,
|
|
/*15_DEVIATN*/ 0x42
|
|
};
|
|
|
|
void frsky2way_init(uint8_t bind)
|
|
{
|
|
//debugln("frsky2way_init");
|
|
FRSKY_init_cc2500(FRSKYD_cc2500_conf);
|
|
|
|
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########
|
|
}
|
|
|
|
void frsky2way_build_bind_packet(void)
|
|
{
|
|
//debugln("build bind");
|
|
//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] = hopping_frequency[idx++];
|
|
packet[7] = hopping_frequency[idx++];
|
|
packet[8] = hopping_frequency[idx++];
|
|
packet[9] = hopping_frequency[idx++];
|
|
packet[10] = hopping_frequency[idx++];
|
|
packet[11] = 0x00;
|
|
packet[12] = 0x00;
|
|
packet[13] = 0x00;
|
|
packet[14] = 0x00;
|
|
packet[15] = 0x00;
|
|
packet[16] = 0x00;
|
|
packet[17] = 0x01;
|
|
}
|
|
|
|
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;//
|
|
#if defined TELEMETRY
|
|
packet[4] = telemetry_counter;
|
|
#else
|
|
packet[4] = 0x00;
|
|
#endif
|
|
|
|
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()
|
|
{
|
|
Frsky_init_hop();
|
|
packet_count=0;
|
|
|
|
if(IS_BIND_IN_PROGRESS)
|
|
{
|
|
state = FRSKY_BIND;
|
|
debugln("initFrSky_2way bind");
|
|
} else {
|
|
state = FRSKY_BIND_DONE;
|
|
debugln("initFrSky_2way bind done");
|
|
}
|
|
return 10000;
|
|
}
|
|
uint16_t ReadFrSky_2way_bind(void)
|
|
{
|
|
frsky2way_build_bind_packet();
|
|
CC2500_Strobe(CC2500_SIDLE);
|
|
CC2500_WriteReg(CC2500_0A_CHANNR, 0x00);
|
|
CC2500_WriteReg(CC2500_23_FSCAL3, 0x89);
|
|
CC2500_Strobe(CC2500_SFRX);//0x3A
|
|
CC2500_WriteData(packet, packet[0]+1);
|
|
|
|
if(IS_BIND_DONE) {
|
|
state = FRSKY_BIND;
|
|
debugln("%s bind done",__func__);
|
|
} else {
|
|
state++;
|
|
}
|
|
|
|
if(state == FRSKY_BIND_DONE) {
|
|
/* reset state again */
|
|
state = 0;
|
|
debugln("%s bind done fr",__func__);
|
|
}
|
|
return 9000;
|
|
}
|
|
|
|
uint16_t ReadFrSky_2way()
|
|
{
|
|
if (state <= FRSKY_BIND_DONE) {
|
|
|
|
frsky2way_init(0);
|
|
//debugln("%s bind done",__func__);
|
|
|
|
state = FRSKY_DATA2;
|
|
counter = 0;
|
|
} else if (state == FRSKY_DATA5) {
|
|
CC2500_Strobe(CC2500_SRX);//0x34 RX enable
|
|
state = FRSKY_DATA1;
|
|
return 9200;
|
|
}
|
|
|
|
counter = (counter + 1) % 188;
|
|
if (state == FRSKY_DATA4) { //telemetry receive
|
|
CC2500_SetTxRxMode(RX_EN);
|
|
CC2500_Strobe(CC2500_SIDLE);
|
|
CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[counter % 47]);
|
|
CC2500_WriteReg(CC2500_23_FSCAL3, 0x89);
|
|
state++;
|
|
return 1300;
|
|
} else {
|
|
if (state == FRSKY_DATA1) {
|
|
len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
|
|
|
//debugln("%d len",len);
|
|
if (len && len<=(0x11+3)) { // 20bytes
|
|
debug("rx tel\n");
|
|
CC2500_ReadData(pkt, len); //received telemetry packets
|
|
#if defined(TELEMETRY)
|
|
if(pkt[len-1] & 0x80)
|
|
{//with valid crc
|
|
debug("rx invalid crc\n");
|
|
packet_count=0;
|
|
frsky_check_telemetry(pkt,len); //check if valid telemetry packets and buffer them.
|
|
}
|
|
debug("rx end crc\n");
|
|
#endif
|
|
} else {
|
|
packet_count++;
|
|
// restart sequence on missed packet - might need count or timeout instead of one missed
|
|
if(packet_count > 100) {//~1sec
|
|
packet_count=0;
|
|
#if defined TELEMETRY
|
|
telemetry_link=0;//no link frames
|
|
pkt[6]=0;//no user frames.
|
|
#endif
|
|
}
|
|
}
|
|
CC2500_SetTxRxMode(TX_EN);
|
|
CC2500_SetPower(); // Set tx_power
|
|
}
|
|
CC2500_Strobe(CC2500_SIDLE);
|
|
CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[counter % 47]);
|
|
|
|
// Frequency offset update
|
|
if ( freq_offset_prev != freq_offset) {
|
|
CC2500_WriteReg(CC2500_0C_FSCTRL0, freq_offset);
|
|
freq_offset_prev = freq_offset;
|
|
}
|
|
|
|
CC2500_WriteReg(CC2500_23_FSCAL3, 0x89);
|
|
CC2500_Strobe(CC2500_SFRX);
|
|
frsky2way_data_frame();
|
|
CC2500_WriteData(packet, packet[0]+1);
|
|
state++;
|
|
}
|
|
return state == FRSKY_DATA4 ? 7500 : 9000;
|
|
}
|
|
|
|
uint16_t convert_channel_frsky(uint8_t num)
|
|
{
|
|
uint16_t val = input.get_channel_data()[num];
|
|
return ((val * 15) >> 4) + 1290;
|
|
}
|
|
|
|
void Frsky_init_hop(void)
|
|
{
|
|
uint8_t val;
|
|
uint8_t channel = rx_tx_addr[0] & 0x07;
|
|
uint8_t channel_spacing = rx_tx_addr[1];
|
|
//Filter bad tables
|
|
if (channel_spacing < 0x02)
|
|
channel_spacing += 0x02;
|
|
if (channel_spacing > 0xE9)
|
|
channel_spacing -= 0xE7;
|
|
if (channel_spacing % 0x2F == 0)
|
|
channel_spacing++;
|
|
|
|
hopping_frequency[0] = channel;
|
|
|
|
for (uint8_t i = 1; i < 50; i++) {
|
|
channel = (channel + channel_spacing) % 0xEB;
|
|
val = channel;
|
|
|
|
if ((val == 0x00) || (val == 0x5A) || (val == 0xDC))
|
|
val++;
|
|
|
|
hopping_frequency[i] = i > 46 ? 0 : val;
|
|
}
|
|
}
|
|
|
|
void FRSKY_init_cc2500(const uint8_t *ptr)
|
|
{
|
|
for (uint8_t i = 0; i < 19; i++)
|
|
{
|
|
uint8_t reg = FRSKY_common_startreg_cc2500_conf[i];
|
|
uint8_t val = ptr[i];
|
|
if (reg == CC2500_0C_FSCTRL0)
|
|
val = freq_offset;
|
|
CC2500_WriteReg(reg, val);
|
|
}
|
|
freq_offset_prev = freq_offset;
|
|
for (uint8_t i = 0; i < 17; i++)
|
|
{
|
|
uint8_t reg = FRSKY_common_end_cc2500_conf[i][0];
|
|
uint8_t val = FRSKY_common_end_cc2500_conf[i][1];
|
|
CC2500_WriteReg(reg, val);
|
|
}
|
|
CC2500_SetTxRxMode(TX_EN);
|
|
CC2500_SetPower();
|
|
CC2500_Strobe(CC2500_SIDLE); // Go to idle...
|
|
}
|
|
|
|
void set_rx_tx_addr(uint32_t id)
|
|
{ // Used by almost all protocols
|
|
rx_tx_addr[0] = (id >> 24) & 0xFF;
|
|
rx_tx_addr[1] = (id >> 16) & 0xFF;
|
|
rx_tx_addr[2] = (id >> 8) & 0xFF;
|
|
rx_tx_addr[3] = (id >> 0) & 0xFF;
|
|
rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F);
|
|
}
|