diff --git a/Multiprotocol/FrSkyDVX_common.ino b/Multiprotocol/FrSkyDVX_common.ino index 801b6e9..9293634 100644 --- a/Multiprotocol/FrSkyDVX_common.ino +++ b/Multiprotocol/FrSkyDVX_common.ino @@ -74,7 +74,7 @@ void Frsky_init_hop(void) /******************************/ /** FrSky V, D and X routines **/ /******************************/ -#if defined(FRSKYV_CC2500_INO) || defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) +#if defined(FRSKYV_CC2500_INO) || defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) || defined(FRSKY_RX_CC2500_INO) const PROGMEM uint8_t FRSKY_common_startreg_cc2500_conf[]= { CC2500_02_IOCFG0 , CC2500_00_IOCFG2 , @@ -119,7 +119,7 @@ void Frsky_init_hop(void) /*15_DEVIATN*/ 0x41 }; #endif - #if defined(FRSKYD_CC2500_INO) + #if defined(FRSKYD_CC2500_INO) || defined(FRSKY_RX_CC2500_INO) const PROGMEM uint8_t FRSKYD_cc2500_conf[]= { /*02_IOCFG0*/ 0x06 , /*00_IOCFG2*/ 0x06 , @@ -142,7 +142,7 @@ void Frsky_init_hop(void) /*15_DEVIATN*/ 0x42 }; #endif - #if defined(FRSKYX_CC2500_INO) + #if defined(FRSKYX_CC2500_INO) || defined(FRSKY_RX_CC2500_INO) const PROGMEM uint8_t FRSKYX_cc2500_conf[]= { //FRSKYX /*02_IOCFG0*/ 0x06 , diff --git a/Multiprotocol/FrSky_Rx_cc2500.ino b/Multiprotocol/FrSky_Rx_cc2500.ino index e37d1d6..09e3bbf 100644 --- a/Multiprotocol/FrSky_Rx_cc2500.ino +++ b/Multiprotocol/FrSky_Rx_cc2500.ino @@ -17,8 +17,9 @@ #include "iface_cc2500.h" - #define FRSKY_RX_D16FCC_LENGTH (30+2) - #define FRSKY_RX_D16LBT_LENGTH (33+2) + #define FRSKY_RX_D16FCC_LENGTH 32 + #define FRSKY_RX_D16LBT_LENGTH 35 + #define FRSKY_RX_D8_LENGTH 20 enum { FRSKY_RX_TUNE_START, @@ -32,6 +33,7 @@ static uint8_t frsky_rx_disable_lna; static uint8_t frsky_rx_data_started; static int8_t frsky_rx_finetune; + static uint16_t frsky_rx_rc_chan[16]; static void __attribute__((unused)) frsky_rx_strobe_rx() { @@ -42,72 +44,35 @@ static void __attribute__((unused)) frsky_rx_strobe_rx() static void __attribute__((unused)) frsky_rx_initialise() { CC2500_Reset(); - - CC2500_WriteReg(CC2500_02_IOCFG0, 0x01); - CC2500_WriteReg(CC2500_18_MCSM0, 0x18); - CC2500_WriteReg(CC2500_07_PKTCTRL1, 0x04); - CC2500_WriteReg(CC2500_3E_PATABLE, 0xFF); - CC2500_WriteReg(CC2500_0C_FSCTRL0, 0); - CC2500_WriteReg(CC2500_0D_FREQ2, 0x5C); - CC2500_WriteReg(CC2500_13_MDMCFG1, 0x23); - CC2500_WriteReg(CC2500_14_MDMCFG0, 0x7A); - CC2500_WriteReg(CC2500_19_FOCCFG, 0x16); - CC2500_WriteReg(CC2500_1A_BSCFG, 0x6C); - CC2500_WriteReg(CC2500_1B_AGCCTRL2, 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_Strobe(CC2500_SIDLE); + CC2500_WriteReg(CC2500_0A_CHANNR, 0); // bind channel switch (sub_protocol) { case FRSKY_RX_D16FCC: - CC2500_WriteReg(CC2500_17_MCSM1, 0x0C); - CC2500_WriteReg(CC2500_0E_FREQ1, 0x76); - CC2500_WriteReg(CC2500_0F_FREQ0, 0x27); - CC2500_WriteReg(CC2500_06_PKTLEN, 0x1E); - CC2500_WriteReg(CC2500_08_PKTCTRL0, 0x01); - CC2500_WriteReg(CC2500_0B_FSCTRL1, 0x0A); - CC2500_WriteReg(CC2500_10_MDMCFG4, 0x7B); - CC2500_WriteReg(CC2500_11_MDMCFG3, 0x61); - CC2500_WriteReg(CC2500_12_MDMCFG2, 0x13); - CC2500_WriteReg(CC2500_15_DEVIATN, 0x51); + FRSKY_init_cc2500(FRSKYX_cc2500_conf); break; case FRSKY_RX_D16LBT: - CC2500_WriteReg(CC2500_17_MCSM1, 0x0E); - CC2500_WriteReg(CC2500_0E_FREQ1, 0x80); - CC2500_WriteReg(CC2500_0F_FREQ0, 0x00); - CC2500_WriteReg(CC2500_06_PKTLEN, 0x23); - CC2500_WriteReg(CC2500_08_PKTCTRL0, 0x01); - CC2500_WriteReg(CC2500_0B_FSCTRL1, 0x08); - CC2500_WriteReg(CC2500_10_MDMCFG4, 0x7B); - CC2500_WriteReg(CC2500_11_MDMCFG3, 0xF8); - CC2500_WriteReg(CC2500_12_MDMCFG2, 0x03); - CC2500_WriteReg(CC2500_15_DEVIATN, 0x53); + FRSKY_init_cc2500(FRSKYXEU_cc2500_conf); + break; + case FRSKY_RX_D8: + FRSKY_init_cc2500(FRSKYD_cc2500_conf); + CC2500_WriteReg(CC2500_07_PKTCTRL1, 0x05); // always check address + CC2500_WriteReg(CC2500_09_ADDR, 0x03); // bind address + CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); // fixed FSCAL3 ? break; } - frsky_rx_disable_lna = IS_POWER_FLAG_on; CC2500_SetTxRxMode(frsky_rx_disable_lna ? TXRX_OFF : RX_EN); // lna disable / enable - frsky_rx_strobe_rx(); - CC2500_WriteReg(CC2500_0A_CHANNR, 0); // bind channel delayMicroseconds(1000); // wait for RX to activate } static void __attribute__((unused)) frsky_rx_set_channel(uint8_t channel) { CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[channel]); - CC2500_WriteReg(CC2500_25_FSCAL1, calData[channel]); + if(sub_protocol == FRSKY_RX_D8) + CC2500_WriteReg(CC2500_23_FSCAL3, 0x89); + else + CC2500_WriteReg(CC2500_25_FSCAL1, calData[channel]); frsky_rx_strobe_rx(); } @@ -126,6 +91,8 @@ static void __attribute__((unused)) frsky_rx_calibrate() static uint8_t __attribute__((unused)) frskyx_rx_check_crc() { + if (sub_protocol == FRSKY_RX_D8) + return 1; uint8_t limit = packet_length - 4; uint16_t lcrc = FrSkyX_crc(&packet[3], limit - 3); // computed crc uint16_t rcrc = (packet[limit] << 8) | (packet[limit + 1] & 0xff); // received crc @@ -134,39 +101,57 @@ static uint8_t __attribute__((unused)) frskyx_rx_check_crc() static void __attribute__((unused)) frsky_rx_build_telemetry_packet() { - static uint16_t frskyx_rx_rc_chan[16]; - uint16_t pxx_channel[8]; + uint16_t raw_channel[8]; uint32_t bits = 0; uint8_t bitsavailable = 0; uint8_t idx = 0; + uint8_t i; - // decode PXX channels - pxx_channel[0] = ((packet[10] << 8) & 0xF00) | packet[9]; - pxx_channel[1] = ((packet[11] << 4) & 0xFF0) | (packet[10] >> 4); - pxx_channel[2] = ((packet[13] << 8) & 0xF00) | packet[12]; - pxx_channel[3] = ((packet[14] << 4) & 0xFF0) | (packet[13] >> 4); - pxx_channel[4] = ((packet[16] << 8) & 0xF00) | packet[15]; - pxx_channel[5] = ((packet[17] << 4) & 0xFF0) | (packet[16] >> 4); - pxx_channel[6] = ((packet[19] << 8) & 0xF00) | packet[18]; - pxx_channel[7] = ((packet[20] << 4) & 0xFF0) | (packet[19] >> 4); - for (unsigned i = 0; i < 8; i++) { - uint8_t shifted = (pxx_channel[i] & 0x800)>0; - uint16_t channel_value = pxx_channel[i] & 0x7FF; - if (channel_value < 64) - frskyx_rx_rc_chan[shifted ? i + 8 : i] = 0; - else - frskyx_rx_rc_chan[shifted ? i + 8 : i] = min(((channel_value - 64) << 4) / 15, 2047); + if (sub_protocol == FRSKY_RX_D16FCC || sub_protocol == FRSKY_RX_D16LBT) { + // decode D16 channels + raw_channel[0] = ((packet[10] << 8) & 0xF00) | packet[9]; + raw_channel[1] = ((packet[11] << 4) & 0xFF0) | (packet[10] >> 4); + raw_channel[2] = ((packet[13] << 8) & 0xF00) | packet[12]; + raw_channel[3] = ((packet[14] << 4) & 0xFF0) | (packet[13] >> 4); + raw_channel[4] = ((packet[16] << 8) & 0xF00) | packet[15]; + raw_channel[5] = ((packet[17] << 4) & 0xFF0) | (packet[16] >> 4); + raw_channel[6] = ((packet[19] << 8) & 0xF00) | packet[18]; + raw_channel[7] = ((packet[20] << 4) & 0xFF0) | (packet[19] >> 4); + for (i = 0; i < 8; i++) { + uint8_t shifted = (raw_channel[i] & 0x800)>0; + uint16_t channel_value = raw_channel[i] & 0x7FF; + if (channel_value < 64) + frsky_rx_rc_chan[shifted ? i + 8 : i] = 0; + else + frsky_rx_rc_chan[shifted ? i + 8 : i] = min(((channel_value - 64) << 4) / 15, 2047); + } + } + else { + // decode D8 channels + raw_channel[0] = ((packet[10] & 0x0F) << 8 | packet[6]); + raw_channel[1] = ((packet[10] & 0xF0) << 4 | packet[7]); + raw_channel[2] = ((packet[11] & 0x0F) << 8 | packet[8]); + raw_channel[3] = ((packet[11] & 0xF0) << 4 | packet[9]); + raw_channel[4] = ((packet[16] & 0x0F) << 8 | packet[12]); + raw_channel[5] = ((packet[16] & 0xF0) << 4 | packet[13]); + raw_channel[6] = ((packet[17] & 0x0F) << 8 | packet[14]); + raw_channel[7] = ((packet[17] & 0xF0) << 4 | packet[15]); + for (i = 0; i < 8; i++) { + if (raw_channel[i] < 1290) + raw_channel[i] = 1290; + frsky_rx_rc_chan[i] = min(((raw_channel[i] - 1290) << 4) / 15, 2047); + } } // buid telemetry packet packet_in[idx++] = RX_LQI; packet_in[idx++] = RX_RSSI; packet_in[idx++] = 0; // start channel - packet_in[idx++] = 16; // number of channels in packet + packet_in[idx++] = sub_protocol == FRSKY_RX_D8 ? 8 : 16; // number of channels in packet // pack channels - for (int i = 0; i < 16; i++) { - bits |= ((uint32_t)frskyx_rx_rc_chan[i]) << bitsavailable; + for (i = 0; i < packet_in[3]; i++) { + bits |= ((uint32_t)frsky_rx_rc_chan[i]) << bitsavailable; bitsavailable += 11; while (bitsavailable >= 8) { packet_in[idx++] = bits & 0xff; @@ -178,6 +163,8 @@ static void __attribute__((unused)) frsky_rx_build_telemetry_packet() uint16_t initFrSky_Rx() { + const uint8_t frsky_rx_length[] = {FRSKY_RX_D16FCC_LENGTH, FRSKY_RX_D16LBT_LENGTH, FRSKY_RX_D8_LENGTH}; + packet_length = frsky_rx_length[sub_protocol]; frsky_rx_initialise(); state = 0; frsky_rx_chanskip = 1; @@ -185,6 +172,8 @@ uint16_t initFrSky_Rx() frsky_rx_data_started = 0; frsky_rx_finetune = 0; telemetry_link = 0; + for(uint8_t ch=0; ch= packet_length) { CC2500_ReadData(packet, packet_length); - if (packet[1] == rx_tx_addr[0] && packet[2] == rx_tx_addr[1] && packet[6] == rx_tx_addr[2] && frskyx_rx_check_crc()) { + if (packet[1] == rx_tx_addr[0] && packet[2] == rx_tx_addr[1] && frskyx_rx_check_crc() && (sub_protocol == FRSKY_RX_D8 || packet[6] == rx_tx_addr[2])) { RX_RSSI = packet[packet_length-2]; if(RX_RSSI >= 128) RX_RSSI -= 128; else RX_RSSI += 128; // hop to next channel - frsky_rx_chanskip = ((packet[4] & 0xC0) >> 6) | ((packet[5] & 0x3F) << 2); + if (sub_protocol == FRSKY_RX_D16FCC || sub_protocol == FRSKY_RX_D16LBT) + frsky_rx_chanskip = ((packet[4] & 0xC0) >> 6) | ((packet[5] & 0x3F) << 2); hopping_frequency_no = (hopping_frequency_no + frsky_rx_chanskip) % 47; frsky_rx_set_channel(hopping_frequency_no); - if(packet[7] == 0 && telemetry_link == 0) { // standard packet, send channels to TX + if((sub_protocol == FRSKY_RX_D8 || packet[7] == 0) && telemetry_link == 0) { // send channels to TX frsky_rx_build_telemetry_packet(); telemetry_link = 1; } diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index b20b409..e867a25 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -814,9 +814,9 @@ Serial: 100000 Baud 8e2 _ xxxx xxxx p -- sub_protocol==TRAXXAS RX6519 0 sub_protocol==FRSKY_RX - D16FCC 0 - D16LBT 1 - D8 2 + FRSKY_RX_D16FCC 0 + FRSKY_RX_D16LBT 1 + FRSKY_RX_D8 2 Power value => 0x80 0=High/1=Low Stream[3] = option_protocol;