diff --git a/Multiprotocol/A7105_SPI.ino b/Multiprotocol/A7105_SPI.ino
new file mode 100644
index 0000000..f848739
--- /dev/null
+++ b/Multiprotocol/A7105_SPI.ino
@@ -0,0 +1,254 @@
+/*
+ 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 .
+ */
+
+//-------------------------------
+//-------------------------------
+//A7105 SPI routines
+//-------------------------------
+//-------------------------------
+#include "iface_a7105.h"
+
+void A7105_WriteData(uint8_t len, uint8_t channel)
+{
+ uint8_t i;
+ CS_off;
+ A7105_Write(A7105_RST_WRPTR);
+ A7105_Write(0x05);
+ for (i = 0; i < len; i++)
+ A7105_Write(packet[i]);
+ CS_on;
+ A7105_WriteReg(0x0F, channel);
+ A7105_Strobe(A7105_TX);
+}
+
+void A7105_ReadData() {
+ uint8_t i;
+ A7105_Strobe(0xF0); //A7105_RST_RDPTR
+ CS_off;
+ A7105_Write(0x45);
+ for (i=0;i<16;i++)
+ packet[i]=A7105_Read();
+ CS_on;
+}
+
+void A7105_WriteReg(uint8_t address, uint8_t data) {
+ CS_off;
+ A7105_Write(address);
+ NOP();
+ A7105_Write(data);
+ CS_on;
+}
+
+uint8_t A7105_ReadReg(uint8_t address) {
+ uint8_t result;
+ CS_off;
+ A7105_Write(address |=0x40); //bit 6 =1 for reading
+ result = A7105_Read();
+ CS_on;
+ return(result);
+}
+
+void A7105_Write(uint8_t command) {
+ uint8_t n=8;
+
+ SCK_off;//SCK start low
+ SDI_off;
+ while(n--) {
+ if(command&0x80)
+ SDI_on;
+ else
+ SDI_off;
+ SCK_on;
+ NOP();
+ SCK_off;
+ command = command << 1;
+ }
+ SDI_on;
+}
+
+uint8_t A7105_Read(void) {
+ uint8_t result=0;
+ uint8_t i;
+
+ SDI_SET_INPUT;
+ for(i=0;i<8;i++) {
+ if(SDI_1) ///if SDIO =1
+ result=(result<<1)|0x01;
+ else
+ result=result<<1;
+ SCK_on;
+ NOP();
+ SCK_off;
+ NOP();
+ }
+ SDI_SET_OUTPUT;
+ return result;
+}
+
+//------------------------
+void A7105_SetTxRxMode(uint8_t mode)
+{
+ if(mode == TX_EN) {
+ A7105_WriteReg(A7105_0B_GPIO1_PIN1, 0x33);
+ A7105_WriteReg(A7105_0C_GPIO2_PIN_II, 0x31);
+ } else if (mode == RX_EN) {
+ A7105_WriteReg(A7105_0B_GPIO1_PIN1, 0x31);
+ A7105_WriteReg(A7105_0C_GPIO2_PIN_II, 0x33);
+ } else {
+ //The A7105 seems to some with a cross-wired power-amp (A7700)
+ //On the XL7105-D03, TX_EN -> RXSW and RX_EN -> TXSW
+ //This means that sleep mode is wired as RX_EN = 1 and TX_EN = 1
+ //If there are other amps in use, we'll need to fix this
+ A7105_WriteReg(A7105_0B_GPIO1_PIN1, 0x33);
+ A7105_WriteReg(A7105_0C_GPIO2_PIN_II, 0x33);
+ }
+}
+
+//------------------------
+uint8_t A7105_Reset()
+{
+ uint8_t result;
+
+ delay(10); //wait 10ms for A7105 wakeup
+ A7105_WriteReg(0x00, 0x00);
+ delay(1000);
+ A7105_SetTxRxMode(TXRX_OFF); //Set both GPIO as output and low
+ result=A7105_ReadReg(0x10) == 0x9E; //check if is reset.
+ A7105_Strobe(A7105_STANDBY);
+ return result;
+}
+
+void A7105_WriteID(uint32_t ida) {
+ CS_off;
+ A7105_Write(0x06);//ex id=0x5475c52a ;txid3txid2txid1txid0
+ A7105_Write((ida>>24)&0xff);//53
+ A7105_Write((ida>>16)&0xff);//75
+ A7105_Write((ida>>8)&0xff);//c5
+ A7105_Write((ida>>0)&0xff);//2a
+ CS_on;
+}
+
+void A7105_SetPower_Value(int power)
+{
+ /*
+ Power amp is ~+16dBm so:
+ TXPOWER_100uW = -23dBm == PAC=0 TBG=0
+ TXPOWER_300uW = -20dBm == PAC=0 TBG=1
+ TXPOWER_1mW = -16dBm == PAC=0 TBG=2
+ TXPOWER_3mW = -11dBm == PAC=0 TBG=4
+ TXPOWER_10mW = -6dBm == PAC=1 TBG=5
+ TXPOWER_30mW = 0dBm == PAC=2 TBG=7
+ TXPOWER_100mW = 1dBm == PAC=3 TBG=7
+ TXPOWER_150mW = 1dBm == PAC=3 TBG=7
+ */
+ uint8_t pac, tbg;
+ switch(power) {
+ case 0: pac = 0; tbg = 0; break;
+ case 1: pac = 0; tbg = 1; break;
+ case 2: pac = 0; tbg = 2; break;
+ case 3: pac = 0; tbg = 4; break;
+ case 4: pac = 1; tbg = 5; break;
+ case 5: pac = 2; tbg = 7; break;
+ case 6: pac = 3; tbg = 7; break;
+ case 7: pac = 3; tbg = 7; break;
+ default: pac = 0; tbg = 0; break;
+ };
+ A7105_WriteReg(0x28, (pac << 3) | tbg);
+}
+
+void A7105_SetPower()
+{
+ uint8_t power=A7105_BIND_POWER;
+ if(IS_BIND_DONE_on)
+ power=IS_POWER_FLAG_on?A7105_HIGH_POWER:A7105_LOW_POWER;
+ else
+ if(IS_RANGE_FLAG_on)
+ power=A7105_POWER_0;
+ A7105_WriteReg(0x28, power);
+}
+
+void A7105_Strobe(uint8_t address) {
+ CS_off;
+ A7105_Write(address);
+ CS_on;
+}
+
+const uint8_t PROGMEM HUBSAN_A7105_regs[] = {
+ 0xFF, 0x63, 0xFF, 0x0F, 0xFF, 0xFF, 0xFF ,0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x05, 0x04, 0xFF,
+ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x2B, 0xFF, 0xFF, 0x62, 0x80, 0xFF, 0xFF, 0x0A, 0xFF, 0xFF, 0x07,
+ 0x17, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x47, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+ 0xFF, 0xFF, 0xFF
+};
+const uint8_t PROGMEM FLYSKY_A7105_regs[] = {
+ 0xff, 0x42, 0x00, 0x14, 0x00, 0xff, 0xff ,0x00, 0x00, 0x00, 0x00, 0x01, 0x21, 0x05, 0x00, 0x50,
+ 0x9e, 0x4b, 0x00, 0x02, 0x16, 0x2b, 0x12, 0x00, 0x62, 0x80, 0x80, 0x00, 0x0a, 0x32, 0xc3, 0x0f,
+ 0x13, 0xc3, 0x00, 0xff, 0x00, 0x00, 0x3b, 0x00, 0x17, 0x47, 0x80, 0x03, 0x01, 0x45, 0x18, 0x00,
+ 0x01, 0x0f, 0xff
+};
+void A7105_Init(uint8_t protocol)
+{
+ void *A7105_Regs;
+
+ if(protocol==INIT_FLYSKY)
+ {
+ A7105_WriteID(0x5475c52A);//0x2Ac57554
+ A7105_Regs=(void *)FLYSKY_A7105_regs;
+ }
+ else
+ {
+ A7105_WriteID(0x55201041);
+ A7105_Regs=(void *)HUBSAN_A7105_regs;
+ }
+ for (uint8_t i = 0; i < 0x33; i++){
+ if( pgm_read_byte_near((uint16_t)(A7105_Regs)+i) != 0xFF)
+ A7105_WriteReg(i, pgm_read_byte_near((uint16_t)(A7105_Regs)+i));
+ }
+ A7105_Strobe(A7105_STANDBY);
+
+ //IF Filter Bank Calibration
+ A7105_WriteReg(A7105_02_CALC,1);
+ while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
+// A7105_ReadReg(A7105_22_IF_CALIB_I);
+// A7105_ReadReg(A7105_24_VCO_CURCAL);
+
+ if(protocol==INIT_FLYSKY)
+ {
+ //VCO Current Calibration
+ A7105_WriteReg(A7105_24_VCO_CURCAL,0x13); //Recommended calibration from A7105 Datasheet
+ //VCO Bank Calibration
+ A7105_WriteReg(A7105_26_VCO_SBCAL_II,0x3b); //Recommended calibration from A7105 Datasheet
+ }
+
+ //VCO Bank Calibrate channel 0
+ A7105_WriteReg(A7105_0F_CHANNEL, 0);
+ A7105_WriteReg(A7105_02_CALC,2);
+ while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
+// A7105_ReadReg(A7105_25_VCO_SBCAL_I);
+
+ //VCO Bank Calibrate channel A0
+ A7105_WriteReg(A7105_0F_CHANNEL, 0xa0);
+ A7105_WriteReg(A7105_02_CALC, 2);
+ while(A7105_ReadReg(A7105_02_CALC)); // Wait for calibration to end
+// A7105_ReadReg(A7105_25_VCO_SBCAL_I);
+
+ //Reset VCO Band calibration
+ if(protocol==INIT_FLYSKY)
+ A7105_WriteReg(A7105_25_VCO_SBCAL_I,0x08);
+
+ A7105_SetTxRxMode(TX_EN);
+ A7105_SetPower();
+
+ A7105_Strobe(A7105_STANDBY);
+}
diff --git a/Multiprotocol/Bayang_nrf24l01.ino b/Multiprotocol/Bayang_nrf24l01.ino
new file mode 100644
index 0000000..bafcb3c
--- /dev/null
+++ b/Multiprotocol/Bayang_nrf24l01.ino
@@ -0,0 +1,177 @@
+/*
+ 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 .
+ */
+// compatible with EAchine H8 mini, H10, BayangToys X6/X7/X9, JJRC JJ850 ...
+
+#if defined(BAYANG_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+#define BAYANG_BIND_COUNT 1000
+#define BAYANG_PACKET_PERIOD 2000
+#define BAYANG_INITIAL_WAIT 500
+#define BAYANG_PACKET_SIZE 15
+#define BAYANG_RF_NUM_CHANNELS 4
+#define BAYANG_RF_BIND_CHANNEL 0
+#define BAYANG_ADDRESS_LENGTH 5
+
+enum BAYANG_FLAGS {
+ // flags going to packet[2]
+ BAYANG_FLAG_RTH = 0x01,
+ BAYANG_FLAG_HEADLESS = 0x02,
+ BAYANG_FLAG_FLIP = 0x08
+};
+
+enum BAYANG_PHASES {
+ BAYANG_BIND = 0,
+ BAYANG_DATA
+};
+
+void BAYANG_send_packet(uint8_t bind)
+{
+ uint8_t i;
+ if (bind)
+ {
+ packet[0]= 0xA4;
+ for(i=0;i<5;i++)
+ packet[i+1]=rx_tx_addr[i];
+ for(i=0;i<4;i++)
+ packet[i+6]=hopping_frequency[i];
+ packet[10] = rx_tx_addr[0];
+ packet[11] = rx_tx_addr[1];
+ }
+ else
+ {
+ uint16_t val;
+ packet[0] = 0xA5;
+ packet[1] = 0xFA; // normal mode is 0xf7, expert 0xfa
+
+ //Flags
+ packet[2] =0x00;
+ if(Servo_data[AUX1] > PPM_SWITCH)
+ packet[2] |= BAYANG_FLAG_FLIP;
+ if(Servo_data[AUX2] > PPM_SWITCH)
+ packet[2] |= BAYANG_FLAG_HEADLESS;
+ if(Servo_data[AUX3] > PPM_SWITCH)
+ packet[2] |= BAYANG_FLAG_RTH;
+
+ packet[3] = 0x00;
+
+ //Aileron
+ val = convert_channel_10b(AILERON);
+ packet[4] = (val>>8) + ((val>>2) & 0xFC);
+ packet[5] = val & 0xFF;
+ //Elevator
+ val = convert_channel_10b(ELEVATOR);
+ packet[6] = (val>>8) + ((val>>2) & 0xFC);
+ packet[7] = val & 0xFF;
+ //Throttle
+ val = convert_channel_10b(THROTTLE);
+ packet[8] = (val>>8) + 0x7C;
+ packet[9] = val & 0xFF;
+ //Rudder
+ val = convert_channel_10b(RUDDER);
+ packet[10] = (val>>8) + (val>>2 & 0xFC);
+ packet[11] = val & 0xFF;
+ }
+ packet[12] = rx_tx_addr[2];
+ packet[13] = 0x0A;
+ packet[14] = 0;
+ for (uint8_t i=0; i < BAYANG_PACKET_SIZE-1; i++)
+ packet[14] += packet[i];
+
+ // Power on, TX mode, 2byte CRC
+ // Why CRC0? xn297 does not interpret it - either 16-bit CRC or nothing
+ XN297_Configure(BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO) | BV(NRF24L01_00_PWR_UP));
+
+ if (bind)
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, BAYANG_RF_BIND_CHANNEL);
+ else
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no++]);
+ hopping_frequency_no%=BAYANG_RF_NUM_CHANNELS;
+
+ // clear packet status bits and TX FIFO
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);
+ NRF24L01_FlushTx();
+ XN297_WritePayload(packet, BAYANG_PACKET_SIZE);
+
+ NRF24L01_SetPower(); // Set tx_power
+}
+
+void BAYANG_init()
+{
+ NRF24L01_Initialize();
+ NRF24L01_SetTxRxMode(TX_EN);
+
+ XN297_SetTXAddr((uint8_t *)"\x00\x00\x00\x00\x00", BAYANG_ADDRESS_LENGTH);
+
+ NRF24L01_FlushTx();
+ NRF24L01_FlushRx();
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowldgement on all data pipes
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0 only
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03);
+ NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x00); // no retransmits
+ NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
+ NRF24L01_SetPower();
+
+ NRF24L01_Activate(0x73); // Activate feature register
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes
+ NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x01);
+ NRF24L01_Activate(0x73);
+}
+
+uint16_t BAYANG_callback()
+{
+ switch (phase)
+ {
+ case BAYANG_BIND:
+ if (bind_counter == 0)
+ {
+ XN297_SetTXAddr(rx_tx_addr, BAYANG_ADDRESS_LENGTH);
+ phase = BAYANG_DATA;
+ BIND_DONE;
+ }
+ else
+ {
+ BAYANG_send_packet(1);
+ bind_counter--;
+ }
+ break;
+ case BAYANG_DATA:
+ BAYANG_send_packet(0);
+ break;
+ }
+ return BAYANG_PACKET_PERIOD;
+}
+
+void BAYANG_initialize_txid()
+{
+ // Strange txid, rx_tx_addr and rf_channels could be anything so I will use on rx_tx_addr for all of them...
+ // Strange also that there is no check of duplicated rf channels... I think we need to implement that later...
+ for(uint8_t i=0; i.
+ */
+//-------------------------------
+//-------------------------------
+//CC2500 SPI routines
+//-------------------------------
+//-------------------------------
+#include "iface_cc2500.h"
+
+void cc2500_readFifo(uint8_t *dpbuffer, uint8_t len)
+{
+ ReadRegisterMulti(CC2500_3F_RXFIFO | CC2500_READ_BURST, dpbuffer, len);
+}
+
+//----------------------
+static void ReadRegisterMulti(uint8_t address, uint8_t data[], uint8_t length)
+{
+ CC25_CSN_off;
+ cc2500_spi_write(address);
+ for(uint8_t i = 0; i < length; i++)
+ data[i] = cc2500_spi_read();
+ CC25_CSN_on;
+}
+
+//*********************************************
+
+void CC2500_WriteRegisterMulti(uint8_t address, const uint8_t data[], uint8_t length)
+{
+ CC25_CSN_off;
+ cc2500_spi_write(CC2500_WRITE_BURST | address);
+ for(uint8_t i = 0; i < length; i++)
+ cc2500_spi_write(data[i]);
+ CC25_CSN_on;
+}
+
+void cc2500_writeFifo(uint8_t *dpbuffer, uint8_t len)
+{
+ cc2500_strobe(CC2500_SFTX);//0x3B
+ CC2500_WriteRegisterMulti(CC2500_3F_TXFIFO, dpbuffer, len);
+ cc2500_strobe(CC2500_STX);//0x35
+}
+
+//--------------------------------------
+void cc2500_spi_write(uint8_t command) {
+ uint8_t n=8;
+
+ SCK_off;//SCK start low
+ SDI_off;
+ while(n--)
+ {
+ if(command&0x80)
+ SDI_on;
+ else
+ SDI_off;
+ SCK_on;
+ NOP();
+ SCK_off;
+ command = command << 1;
+ }
+ SDI_on;
+}
+
+//----------------------------
+void cc2500_writeReg(uint8_t address, uint8_t data) {//same as 7105
+ CC25_CSN_off;
+ cc2500_spi_write(address);
+ NOP();
+ cc2500_spi_write(data);
+ CC25_CSN_on;
+}
+
+uint8_t cc2500_spi_read(void)
+{
+ uint8_t result;
+ uint8_t i;
+ result=0;
+ for(i=0;i<8;i++)
+ {
+ if(SDO_1) ///
+ result=(result<<1)|0x01;
+ else
+ result=result<<1;
+ SCK_on;
+ NOP();
+ SCK_off;
+ NOP();
+ }
+ return result;
+}
+
+//--------------------------------------------
+uint8_t cc2500_readReg(uint8_t address)
+{
+ uint8_t result;
+ CC25_CSN_off;
+ address |=0x80; //bit 7 =1 for reading
+ cc2500_spi_write(address);
+ result = cc2500_spi_read();
+ CC25_CSN_on;
+ return(result);
+}
+//------------------------
+void cc2500_strobe(uint8_t address)
+{
+ CC25_CSN_off;
+ cc2500_spi_write(address);
+ CC25_CSN_on;
+}
+//------------------------
+void cc2500_resetChip(void)
+{
+ // Toggle chip select signal
+ CC25_CSN_on;
+ _delay_us(30);
+ CC25_CSN_off;
+ _delay_us(30);
+ CC25_CSN_on;
+ _delay_us(45);
+ cc2500_strobe(CC2500_SRES);
+ _delay_ms(100);
+}
+
+uint8_t CC2500_Reset()
+{
+ cc2500_strobe(CC2500_SRES);
+ _delay_us(1000);
+ CC2500_SetTxRxMode(TXRX_OFF);
+ return cc2500_readReg(CC2500_0E_FREQ1) == 0xC4;//check if reset
+}
+
+void CC2500_SetPower_Value(uint8_t power)
+{
+ const unsigned char patable[8]= {
+ 0xC5, // -12dbm
+ 0x97, // -10dbm
+ 0x6E, // -8dbm
+ 0x7F, // -6dbm
+ 0xA9, // -4dbm
+ 0xBB, // -2dbm
+ 0xFE, // 0dbm
+ 0xFF // 1.5dbm
+ };
+ if (power > 7)
+ power = 7;
+ cc2500_writeReg(CC2500_3E_PATABLE, patable[power]);
+}
+
+void CC2500_SetPower()
+{
+ uint8_t power=CC2500_BIND_POWER;
+ if(IS_BIND_DONE_on)
+ power=IS_POWER_FLAG_on?CC2500_HIGH_POWER:CC2500_LOW_POWER;
+ else
+ if(IS_RANGE_FLAG_on)
+ power=CC2500_POWER_0;
+ cc2500_writeReg(CC2500_3E_PATABLE, power);
+}
+
+void CC2500_SetTxRxMode(uint8_t mode)
+{
+ if(mode == TX_EN)
+ {//from deviation firmware
+ cc2500_writeReg(CC2500_02_IOCFG0, 0x2F | 0x40);
+ cc2500_writeReg(CC2500_00_IOCFG2, 0x2F);
+ }
+ else
+ if (mode == RX_EN)
+ {
+ cc2500_writeReg(CC2500_02_IOCFG0, 0x2F);
+ cc2500_writeReg(CC2500_00_IOCFG2, 0x2F | 0x40);
+ }
+ else
+ {
+ cc2500_writeReg(CC2500_02_IOCFG0, 0x2F);
+ cc2500_writeReg(CC2500_00_IOCFG2, 0x2F);
+ }
+}
\ No newline at end of file
diff --git a/Multiprotocol/CG023_nrf24l01.ino b/Multiprotocol/CG023_nrf24l01.ino
new file mode 100644
index 0000000..62de5c1
--- /dev/null
+++ b/Multiprotocol/CG023_nrf24l01.ino
@@ -0,0 +1,194 @@
+/*
+ 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 .
+ */
+// compatible with EAchine 3D X4, CG023/CG031, Attop YD-822/YD-829/YD-829C
+
+#if defined(CG023_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+#define CG023_PACKET_PERIOD 8200 // Timeout for callback in uSec
+#define CG023_INITIAL_WAIT 500
+#define CG023_PACKET_SIZE 15 // packets have 15-byte payload
+#define CG023_RF_BIND_CHANNEL 0x2D
+#define CG023_BIND_COUNT 800 // 6 seconds
+#define YD829_PACKET_PERIOD 4100 // Timeout for callback in uSec
+
+enum CG023_FLAGS {
+ // flags going to packet[13]
+ CG023_FLAG_FLIP = 0x01,
+ CG023_FLAG_EASY = 0x02,
+ CG023_FLAG_VIDEO = 0x04,
+ CG023_FLAG_STILL = 0x08,
+ CG023_FLAG_LED_OFF = 0x10,
+ CG023_FLAG_RATE_LOW = 0x00,
+ CG023_FLAG_RATE_MID = 0x20,
+ CG023_FLAG_RATE_HIGH= 0x40,
+};
+
+enum YD829_FLAGS {
+ // flags going to packet[13] (YD-829)
+ YD829_FLAG_FLIP = 0x01,
+ YD829_MASK_RATE = 0x0C,
+ YD829_FLAG_RATE_MID = 0x04,
+ YD829_FLAG_RATE_HIGH= 0x08,
+ YD829_FLAG_HEADLESS = 0x20,
+ YD829_FLAG_VIDEO = 0x40,
+ YD829_FLAG_STILL = 0x80,
+};
+
+enum CG023_PHASES {
+ CG023_BIND = 0,
+ CG023_DATA
+};
+
+void CG023_send_packet(uint8_t bind)
+{
+ if (bind)
+ packet[0]= 0xaa;
+ else
+ packet[0]= 0x55;
+ // transmitter id
+ packet[1] = rx_tx_addr[0];
+ packet[2] = rx_tx_addr[1];
+ // unknown
+ packet[3] = 0x00;
+ packet[4] = 0x00;
+ // throttle : 0x00 - 0xFF
+ packet[5] = convert_channel_8b(THROTTLE);
+ // rudder
+ packet[6] = convert_channel_8b_scale(RUDDER,0x44,0xBC); // yaw right : 0x80 (neutral) - 0xBC (right)
+ if (packet[6]<=0x80)
+ packet[6]=0x80-packet[6]; // yaw left : 0x00 (neutral) - 0x3C (left)
+ // elevator : 0xBB - 0x7F - 0x43
+ packet[7] = convert_channel_8b_scale(ELEVATOR, 0x43, 0xBB);
+ // aileron : 0x43 - 0x7F - 0xBB
+ packet[8] = convert_channel_8b_scale(AILERON, 0x43, 0xBB);
+ // throttle trim : 0x30 - 0x20 - 0x10
+ packet[9] = 0x20; // neutral
+ // neutral trims
+ packet[10] = 0x20;
+ packet[11] = 0x40;
+ packet[12] = 0x40;
+ if(sub_protocol==CG023)
+ {
+ // rate
+ packet[13] = CG023_FLAG_RATE_HIGH;
+ // flags
+ if(Servo_data[AUX1] > PPM_SWITCH)
+ packet[13] |= CG023_FLAG_FLIP;
+ if(Servo_data[AUX2] > PPM_SWITCH)
+ packet[13] |= CG023_FLAG_LED_OFF;
+ if(Servo_data[AUX3] > PPM_SWITCH)
+ packet[13] |= CG023_FLAG_STILL;
+ if(Servo_data[AUX4] > PPM_SWITCH)
+ packet[13] |= CG023_FLAG_VIDEO;
+ if(Servo_data[AUX5] > PPM_SWITCH)
+ packet[13] |= CG023_FLAG_EASY;
+ }
+ else
+ {// YD829
+ // rate
+ packet[13] = YD829_FLAG_RATE_HIGH;
+ // flags
+ if(Servo_data[AUX1] > PPM_SWITCH)
+ packet[13] |= YD829_FLAG_FLIP;
+ if(Servo_data[AUX3] > PPM_SWITCH)
+ packet[13] |= YD829_FLAG_STILL;
+ if(Servo_data[AUX4] > PPM_SWITCH)
+ packet[13] |= YD829_FLAG_VIDEO;
+ if(Servo_data[AUX5] > PPM_SWITCH)
+ packet[13] |= YD829_FLAG_HEADLESS;
+ }
+ packet[14] = 0;
+
+ // Power on, TX mode, 2byte CRC
+ // Why CRC0? xn297 does not interpret it - either 16-bit CRC or nothing
+ XN297_Configure(BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO) | BV(NRF24L01_00_PWR_UP));
+ if (bind)
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, CG023_RF_BIND_CHANNEL);
+ else
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency_no);
+ // clear packet status bits and TX FIFO
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);
+ NRF24L01_FlushTx();
+ XN297_WritePayload(packet, CG023_PACKET_SIZE);
+
+ NRF24L01_SetPower(); // Set tx_power
+}
+
+void CG023_init()
+{
+ NRF24L01_Initialize();
+ NRF24L01_SetTxRxMode(TX_EN);
+ XN297_SetTXAddr((uint8_t *)"\x26\xA8\x67\x35\xCC", 5);
+
+ NRF24L01_FlushTx();
+ NRF24L01_FlushRx();
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowldgement on all data pipes
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0 only
+ NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
+ NRF24L01_SetPower();
+}
+
+uint16_t CG023_callback()
+{
+ switch (phase)
+ {
+ case CG023_BIND:
+ if (bind_counter == 0)
+ {
+ phase = CG023_DATA;
+ BIND_DONE;
+ }
+ else
+ {
+ CG023_send_packet(1);
+ bind_counter--;
+ }
+ break;
+ case CG023_DATA:
+ CG023_send_packet(0);
+ break;
+ }
+ if(sub_protocol==CG023)
+ return CG023_PACKET_PERIOD;
+ else
+ return YD829_PACKET_PERIOD;
+}
+
+void CG023_initialize_txid()
+{
+ rx_tx_addr[0]= 0x80 | (rx_tx_addr[0] % 0x40);
+ if( rx_tx_addr[0] == 0xAA) // avoid using same freq for bind and data channel
+ rx_tx_addr[0] ++;
+
+ hopping_frequency_no = rx_tx_addr[0] - 0x7D; // rf channel for data packets
+}
+
+uint16_t initCG023(void)
+{
+ BIND_IN_PROGRESS; // autobind protocol
+ bind_counter = CG023_BIND_COUNT;
+ CG023_initialize_txid();
+ CG023_init();
+ phase=CG023_BIND;
+ if(sub_protocol==CG023)
+ return CG023_INITIAL_WAIT+CG023_PACKET_PERIOD;
+ else
+ return CG023_INITIAL_WAIT+YD829_PACKET_PERIOD;
+}
+
+#endif
diff --git a/Multiprotocol/CX10_nrf24l01.ino b/Multiprotocol/CX10_nrf24l01.ino
new file mode 100644
index 0000000..4525b7d
--- /dev/null
+++ b/Multiprotocol/CX10_nrf24l01.ino
@@ -0,0 +1,214 @@
+/*
+ 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 .
+ */
+// compatible with Cheerson CX-10 blue & newer red pcb, CX-10A, CX11, CX-10 green pcb, DM007, Floureon FX-10, CX-Stars
+
+#if defined(CX10_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+#define CX10_BIND_COUNT 4360 // 6 seconds
+#define CX10_PACKET_SIZE 15
+#define CX10A_PACKET_SIZE 19 // CX10 blue board packets have 19-byte payload
+#define CX10_PACKET_PERIOD 1316 // Timeout for callback in uSec
+#define CX10A_PACKET_PERIOD 6000
+
+#define INITIAL_WAIT 500
+
+// flags
+#define CX10_FLAG_FLIP 0x10 // goes to rudder channel
+#define CX10_FLAG_MODE_MASK 0x03
+#define CX10_FLAG_HEADLESS 0x04
+// flags2
+#define CX10_FLAG_VIDEO 0x02
+#define CX10_FLAG_SNAPSHOT 0x04
+
+// frequency channel management
+#define RF_BIND_CHANNEL 0x02
+#define NUM_RF_CHANNELS 4
+
+enum {
+ CX10_INIT1 = 0,
+ CX10_BIND1,
+ CX10_BIND2,
+ CX10_DATA
+};
+
+void CX10_Write_Packet(uint8_t bind)
+{
+ uint8_t offset = 0;
+ if(sub_protocol == CX10_BLUE)
+ offset = 4;
+ packet[0] = bind ? 0xAA : 0x55;
+ packet[1] = rx_tx_addr[0];
+ packet[2] = rx_tx_addr[1];
+ packet[3] = rx_tx_addr[2];
+ packet[4] = rx_tx_addr[3];
+ // packet[5] to [8] (aircraft id) is filled during bind for blue board
+ packet[5+offset] = lowByte(Servo_data[AILERON]);
+ packet[6+offset]= highByte(Servo_data[AILERON]);
+ packet[7+offset]= lowByte(Servo_data[ELEVATOR]);
+ packet[8+offset]= highByte(Servo_data[ELEVATOR]);
+ packet[9+offset]= lowByte(Servo_data[THROTTLE]);
+ packet[10+offset]= highByte(Servo_data[THROTTLE]);
+ packet[11+offset]= lowByte(Servo_data[RUDDER]);
+ packet[12+offset]= highByte(Servo_data[RUDDER]);
+
+ // Channel 5 - flip flag
+ if(Servo_data[AUX1] > PPM_SWITCH)
+ packet[12+offset] |= CX10_FLAG_FLIP; // flip flag
+
+ // Channel 6 - mode
+ if(Servo_data[AUX2] > PPM_MAX_COMMAND) // mode 3 / headless on CX-10A
+ packet[13+offset] = 0x02;
+ else
+ if(Servo_data[AUX2] < PPM_MIN_COMMAND)
+ packet[13+offset] = 0x00; // mode 1
+ else
+ packet[13+offset] = 0x01; // mode 2
+
+ flags=0;
+ if(sub_protocol == DM007)
+ {
+ // Channel 7 - snapshot
+ if(Servo_data[AUX3] > PPM_SWITCH)
+ flags |= CX10_FLAG_SNAPSHOT;
+ // Channel 8 - video
+ if(Servo_data[AUX4] > PPM_SWITCH)
+ flags |= CX10_FLAG_VIDEO;
+ // Channel 9 - headless
+ if(Servo_data[AUX5] > PPM_SWITCH)
+ packet[13+offset] |= CX10_FLAG_HEADLESS;
+ }
+ packet[14+offset] = flags;
+
+ // Power on, TX mode, 2byte CRC
+ // Why CRC0? xn297 does not interpret it - either 16-bit CRC or nothing
+ XN297_Configure(BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO) | BV(NRF24L01_00_PWR_UP));
+ if (bind)
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, RF_BIND_CHANNEL);
+ else
+ {
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no++]);
+ hopping_frequency_no %= NUM_RF_CHANNELS;
+ }
+ // clear packet status bits and TX FIFO
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);
+ NRF24L01_FlushTx();
+
+ XN297_WritePayload(packet, packet_length);
+ NRF24L01_SetPower();
+}
+
+void CX10_init()
+{
+ NRF24L01_Initialize();
+ NRF24L01_SetTxRxMode(TX_EN);
+ XN297_SetTXAddr((uint8_t *)"\xcc\xcc\xcc\xcc\xcc",5);
+ XN297_SetRXAddr((uint8_t *)"\xcc\xcc\xcc\xcc\xcc",5);
+ NRF24L01_FlushTx();
+ NRF24L01_FlushRx();
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowledgment on all data pipes
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable data pipe 0 only
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, packet_length); // rx pipe 0 (used only for blue board)
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, RF_BIND_CHANNEL);
+ NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
+ NRF24L01_SetPower();
+}
+
+uint16_t CX10_callback() {
+ switch (phase) {
+ case CX10_INIT1:
+ phase = bind_phase;
+ break;
+ case CX10_BIND1:
+ if (bind_counter == 0)
+ {
+ phase = CX10_DATA;
+ BIND_DONE;
+ }
+ else
+ {
+ CX10_Write_Packet(1);
+ bind_counter--;
+ }
+ break;
+ case CX10_BIND2:
+ if( NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_RX_DR))
+ { // RX fifo data ready
+ XN297_ReadPayload(packet, packet_length);
+ NRF24L01_SetTxRxMode(TXRX_OFF);
+ NRF24L01_SetTxRxMode(TX_EN);
+ if(packet[9] == 1)
+ phase = CX10_BIND1;
+ }
+ else
+ {
+ NRF24L01_SetTxRxMode(TXRX_OFF);
+ NRF24L01_SetTxRxMode(TX_EN);
+ CX10_Write_Packet(1);
+ delay(1); // used to be 300µs in deviation but not working so 1ms now
+ // switch to RX mode
+ NRF24L01_SetTxRxMode(TXRX_OFF);
+ NRF24L01_FlushRx();
+ NRF24L01_SetTxRxMode(RX_EN);
+ XN297_Configure(BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO) | BV(NRF24L01_00_PWR_UP) | BV(NRF24L01_00_PRIM_RX));
+ }
+ break;
+ case CX10_DATA:
+ CX10_Write_Packet(0);
+ break;
+ }
+ return packet_period;
+}
+
+void initialize_txid()
+{
+ rx_tx_addr[1]%= 0x30;
+ hopping_frequency[0] = 0x03 + (rx_tx_addr[0] & 0x0F);
+ hopping_frequency[1] = 0x16 + (rx_tx_addr[0] >> 4);
+ hopping_frequency[2] = 0x2D + (rx_tx_addr[1] & 0x0F);
+ hopping_frequency[3] = 0x40 + (rx_tx_addr[1] >> 4);
+}
+
+uint16_t initCX10(void)
+{
+ switch(sub_protocol)
+ {
+ case CX10_GREEN:
+ case DM007:
+ packet_length = CX10_PACKET_SIZE;
+ packet_period = CX10_PACKET_PERIOD;
+ bind_phase = CX10_BIND1;
+ bind_counter = CX10_BIND_COUNT;
+ break;
+ case CX10_BLUE:
+ packet_length = CX10A_PACKET_SIZE;
+ packet_period = CX10A_PACKET_PERIOD;
+ bind_phase = CX10_BIND2;
+ bind_counter=0;
+ for(uint8_t i=0; i<4; i++)
+ packet[5+i] = 0xff; // clear aircraft id
+ packet[9] = 0;
+ break;
+ }
+ initialize_txid();
+ CX10_init();
+ phase = CX10_INIT1;
+ BIND_IN_PROGRESS; // autobind protocol
+ return INITIAL_WAIT;
+}
+
+#endif
diff --git a/Multiprotocol/CYRF6936_SPI.ino b/Multiprotocol/CYRF6936_SPI.ino
new file mode 100644
index 0000000..5ed9109
--- /dev/null
+++ b/Multiprotocol/CYRF6936_SPI.ino
@@ -0,0 +1,284 @@
+/*
+ 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 .
+ */
+#include "iface_cyrf6936.h"
+
+void cyrf_spi_write(uint8_t command)
+{
+ uint8_t n=8;
+ SCK_off;//SCK start low
+ SDI_off;
+ while(n--) {
+ if(command&0x80)
+ SDI_on;
+ else
+ SDI_off;
+ SCK_on;
+ NOP();
+ SCK_off;
+ command = command << 1;
+ }
+ SDI_on;
+}
+
+uint8_t cyrf_spi_read()
+{
+ uint8_t result;
+ uint8_t i;
+ result=0;
+ for(i=0;i<8;i++)
+ {
+ if(SDO_1) ///
+ result=(result<<1)|0x01;
+ else
+ result=result<<1;
+ SCK_on;
+ NOP();
+ SCK_off;
+ NOP();
+ }
+ return result;
+}
+
+void CYRF_WriteRegister(uint8_t address, uint8_t data)
+{
+ CYRF_CSN_off;
+ cyrf_spi_write(0x80 | address);
+ cyrf_spi_write(data);
+ CYRF_CSN_on;
+}
+
+void CYRF_WriteRegisterMulti(uint8_t address, const uint8_t data[], uint8_t length)
+{
+ uint8_t i;
+
+ CYRF_CSN_off;
+ cyrf_spi_write(0x80 | address);
+ for(i = 0; i < length; i++)
+ cyrf_spi_write(data[i]);
+ CYRF_CSN_on;
+}
+
+void CYRF_ReadRegisterMulti(uint8_t address, uint8_t data[], uint8_t length)
+{
+ uint8_t i;
+
+ CYRF_CSN_off;
+ cyrf_spi_write(address);
+ for(i = 0; i < length; i++)
+ data[i] = cyrf_spi_read();
+ CYRF_CSN_on;
+}
+
+uint8_t CYRF_ReadRegister(uint8_t address)
+{
+ uint8_t data;
+ CYRF_CSN_off;
+ cyrf_spi_write(address);
+ data = cyrf_spi_read();
+ CYRF_CSN_on;
+ return data;
+}
+//
+
+uint8_t CYRF_Reset()
+{
+ CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x01);//software reset
+ _delay_us(200);//
+ // RS_HI;
+ // _delay_us(100);
+ // RS_LO;
+ // _delay_us(100);
+ CYRF_WriteRegister(CYRF_0C_XTAL_CTRL, 0xC0); //Enable XOUT as GPIO
+ CYRF_WriteRegister(CYRF_0D_IO_CFG, 0x04); //Enable PACTL as GPIO
+ CYRF_SetTxRxMode(TXRX_OFF);
+ //Verify the CYRD chip is responding
+ return (CYRF_ReadRegister(CYRF_10_FRAMING_CFG) == 0xa5);//return if reset
+}
+
+uint8_t CYRF_MaxPower()
+{
+ return (*((uint8_t*)0x08001007) == 0) ? CYRF_PWR_100MW : CYRF_PWR_10MW;
+}
+/*
+*
+*/
+void CYRF_GetMfgData(uint8_t data[])
+{
+ /* Fuses power on */
+ CYRF_WriteRegister(CYRF_25_MFG_ID, 0xFF);
+
+ CYRF_ReadRegisterMulti(CYRF_25_MFG_ID, data, 6);
+
+ /* Fuses power off */
+ CYRF_WriteRegister(CYRF_25_MFG_ID, 0x00);
+}
+
+/*
+* 1 - Tx else Rx
+*/
+void CYRF_SetTxRxMode(uint8_t mode)
+{
+ //Set the post tx/rx state
+ CYRF_WriteRegister(CYRF_0F_XACT_CFG, mode == TX_EN ? 0x28 : 0x2C); //was 0x2C:0x28 but reversed in last deviation
+ if(mode == TX_EN)
+ CYRF_WriteRegister(CYRF_0E_GPIO_CTRL,0x80);
+ else
+ CYRF_WriteRegister(CYRF_0E_GPIO_CTRL,0x20);
+}
+/*
+*
+*/
+void CYRF_ConfigRFChannel(uint8_t ch)
+{
+ CYRF_WriteRegister(CYRF_00_CHANNEL,ch);
+}
+
+void CYRF_SetPower_Value(uint8_t power)
+{
+ uint8_t val = CYRF_ReadRegister(CYRF_03_TX_CFG) & 0xF8;
+ CYRF_WriteRegister(CYRF_03_TX_CFG, val | (power & 0x07));
+}
+
+void CYRF_SetPower(uint8_t val)
+{
+ uint8_t power=CYRF_BIND_POWER;
+ if(IS_BIND_DONE_on)
+ power=IS_POWER_FLAG_on?CYRF_HIGH_POWER:CYRF_LOW_POWER;
+ else
+ if(IS_RANGE_FLAG_on)
+ power=CYRF_POWER_0;
+ CYRF_WriteRegister(CYRF_03_TX_CFG, val | power);
+}
+
+/*
+*
+*/
+void CYRF_ConfigCRCSeed(uint16_t crc)
+{
+ CYRF_WriteRegister(CYRF_15_CRC_SEED_LSB,crc & 0xff);
+ CYRF_WriteRegister(CYRF_16_CRC_SEED_MSB,crc >> 8);
+}
+/*
+* these are the recommended sop codes from Cyrpress
+* See "WirelessUSB LP/LPstar and PRoC LP/LPstar Technical Reference Manual"
+*/
+void CYRF_ConfigSOPCode(const uint8_t *sopcodes)
+{
+ //NOTE: This can also be implemented as:
+ //for(i = 0; i < 8; i++) WriteRegister)0x23, sopcodes[i];
+ CYRF_WriteRegisterMulti(CYRF_22_SOP_CODE, sopcodes, 8);
+}
+
+void CYRF_ConfigDataCode(const uint8_t *datacodes, uint8_t len)
+{
+ //NOTE: This can also be implemented as:
+ //for(i = 0; i < len; i++) WriteRegister)0x23, datacodes[i];
+ CYRF_WriteRegisterMulti(CYRF_23_DATA_CODE, datacodes, len);
+}
+
+void CYRF_WritePreamble(uint32_t preamble)
+{
+ CYRF_CSN_off;
+ cyrf_spi_write(0x80 | 0x24);
+ cyrf_spi_write(preamble & 0xff);
+ cyrf_spi_write((preamble >> 8) & 0xff);
+ cyrf_spi_write((preamble >> 16) & 0xff);
+ CYRF_CSN_on;
+}
+/*
+*
+*/
+void CYRF_StartReceive()
+{
+ CYRF_WriteRegister(CYRF_05_RX_CTRL,0x87);
+}
+
+void CYRF_ReadDataPacket(uint8_t dpbuffer[])
+{
+ CYRF_ReadRegisterMulti(CYRF_21_RX_BUFFER, dpbuffer, 0x10);
+}
+
+void CYRF_ReadDataPacketLen(uint8_t dpbuffer[], uint8_t length)
+{
+ ReadRegisterMulti(CYRF_21_RX_BUFFER, dpbuffer, length);
+}
+
+void CYRF_WriteDataPacketLen(const uint8_t dpbuffer[], uint8_t len)
+{
+ CYRF_WriteRegister(CYRF_01_TX_LENGTH, len);
+ CYRF_WriteRegister(CYRF_02_TX_CTRL, 0x40);
+ CYRF_WriteRegisterMulti(CYRF_20_TX_BUFFER, dpbuffer, len);
+ CYRF_WriteRegister(CYRF_02_TX_CTRL, 0xBF);
+}
+
+void CYRF_WriteDataPacket(const uint8_t dpbuffer[])
+{
+ CYRF_WriteDataPacketLen(dpbuffer, 16);
+}
+
+uint8_t CYRF_ReadRSSI(uint8_t dodummyread)
+{
+ uint8_t result;
+ if(dodummyread)
+ CYRF_ReadRegister(CYRF_13_RSSI);
+ result = CYRF_ReadRegister(CYRF_13_RSSI);
+ if(result & 0x80)
+ result = CYRF_ReadRegister(CYRF_13_RSSI);
+ return (result & 0x0F);
+}
+
+//NOTE: This routine will reset the CRC Seed
+void CYRF_FindBestChannels(uint8_t *channels, uint8_t len, uint8_t minspace, uint8_t min, uint8_t max)
+{
+ #define NUM_FREQ 80
+ #define FREQ_OFFSET 4
+ uint8_t rssi[NUM_FREQ];
+
+ if (min < FREQ_OFFSET)
+ min = FREQ_OFFSET;
+ if (max > NUM_FREQ)
+ max = NUM_FREQ;
+
+ uint8_t i;
+ int8_t j;
+ memset(channels, 0, sizeof(uint8_t) * len);
+ CYRF_ConfigCRCSeed(0x0000);
+ CYRF_SetTxRxMode(RX_EN);
+ //Wait for pre-amp to switch from send to receive
+ _delay_us(1000);
+ for(i = 0; i < NUM_FREQ; i++)
+ {
+ CYRF_ConfigRFChannel(i);
+ CYRF_ReadRegister(CYRF_13_RSSI);
+ CYRF_StartReceive();
+ _delay_us(10);
+ rssi[i] = CYRF_ReadRegister(CYRF_13_RSSI);
+ }
+
+ for (i = 0; i < len; i++)
+ {
+ channels[i] = min;
+ for (j = min; j < max; j++)
+ if (rssi[j] < rssi[channels[i]])
+ channels[i] = j;
+ for (j = channels[i] - minspace; j < channels[i] + minspace; j++) {
+ //Ensure we don't reuse any channels within minspace of the selected channel again
+ if (j < 0 || j >= NUM_FREQ)
+ continue;
+ rssi[j] = 0xff;
+ }
+ }
+ CYRF_SetTxRxMode(TX_EN);
+}
diff --git a/Multiprotocol/DSM2_cyrf6936.ino b/Multiprotocol/DSM2_cyrf6936.ino
new file mode 100644
index 0000000..676f3cc
--- /dev/null
+++ b/Multiprotocol/DSM2_cyrf6936.ino
@@ -0,0 +1,535 @@
+/*
+ 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 .
+ */
+
+#if defined(DSM2_CYRF6936_INO)
+
+#include "iface_cyrf6936.h"
+
+#define DSM2_NUM_CHANNELS 7
+#define RANDOM_CHANNELS 0 // disabled
+//#define RANDOM_CHANNELS 1 // enabled
+#define BIND_CHANNEL 0x0d //13 This can be any odd channel
+#define NUM_WAIT_LOOPS (100 / 5) //each loop is ~5us. Do not wait more than 100us
+
+//During binding we will send BIND_COUNT/2 packets
+//One packet each 10msec
+#define BIND_COUNT1 600
+
+enum {
+ DSM2_BIND = 0,
+ DSM2_CHANSEL = BIND_COUNT1 + 0,
+ DSM2_CH1_WRITE_A = BIND_COUNT1 + 1,
+ DSM2_CH1_CHECK_A = BIND_COUNT1 + 2,
+ DSM2_CH2_WRITE_A = BIND_COUNT1 + 3,
+ DSM2_CH2_CHECK_A = BIND_COUNT1 + 4,
+ DSM2_CH2_READ_A = BIND_COUNT1 + 5,
+ DSM2_CH1_WRITE_B = BIND_COUNT1 + 6,
+ DSM2_CH1_CHECK_B = BIND_COUNT1 + 7,
+ DSM2_CH2_WRITE_B = BIND_COUNT1 + 8,
+ DSM2_CH2_CHECK_B = BIND_COUNT1 + 9,
+ DSM2_CH2_READ_B = BIND_COUNT1 + 10,
+};
+
+
+const uint8_t pncodes[5][9][8] = {
+ /* Note these are in order transmitted (LSB 1st) */
+ { /* Row 0 */
+ /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8},
+ /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6},
+ /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9},
+ /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4},
+ /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0},
+ /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8},
+ /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D},
+ /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1},
+ /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86}
+ },
+ { /* Row 1 */
+ /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3},
+ /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9},
+ /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82},
+ /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB},
+ /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7},
+ /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95},
+ /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4},
+ /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF},
+ /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}
+ },
+ { /* Row 2 */
+ /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97},
+ /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA},
+ /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE},
+ /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD},
+ /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD},
+ /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9},
+ /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3},
+ /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0},
+ /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}
+ },
+ { /* Row 3 */
+ /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E},
+ /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7},
+ /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1},
+ /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4},
+ /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6},
+ /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80},
+ /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88},
+ /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88},
+ /* Col 8 */ {0x88, 0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40}
+ },
+ { /* Row 4 */
+ /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93},
+ /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C},
+ /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA},
+ /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC},
+ /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84},
+ /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7},
+ /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0},
+ /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1},
+ /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}
+ },
+};
+
+//
+uint8_t chidx;
+uint8_t sop_col;
+uint8_t data_col;
+uint16_t cyrf_state;
+uint8_t crcidx;
+uint8_t binding;
+uint16_t crc;
+uint8_t model;
+
+/*
+#ifdef USE_FIXED_MFGID
+const uint8_t cyrfmfg_id[6] = {0x5e, 0x28, 0xa3, 0x1b, 0x00, 0x00}; //dx8
+const uint8_t cyrfmfg_id[6] = {0xd4, 0x62, 0xd6, 0xad, 0xd3, 0xff}; //dx6i
+#else
+//uint8_t cyrfmfg_id[6];
+#endif
+*/
+
+void build_bind_packet()
+{
+ uint8_t i;
+ uint16_t sum = 384 - 0x10;//
+ packet[0] = crc >> 8;
+ packet[1] = crc & 0xff;
+ packet[2] = 0xff ^ cyrfmfg_id[2];
+ packet[3] = (0xff ^ cyrfmfg_id[3]) + model;
+ packet[4] = packet[0];
+ packet[5] = packet[1];
+ packet[6] = packet[2];
+ packet[7] = packet[3];
+ for(i = 0; i < 8; i++)
+ sum += packet[i];
+ packet[8] = sum >> 8;
+ packet[9] = sum & 0xff;
+ packet[10] = 0x01; //???
+ packet[11] = DSM2_NUM_CHANNELS;
+ if(sub_protocol==DSMX) //DSMX type
+ packet[12] = 0xb2; // Telemetry off: packet[12] = num_channels < 8 && Model.proto_opts[PROTOOPTS_TELEMETRY] == TELEM_OFF ? 0xa2 : 0xb2;
+ else
+#if DSM2_NUM_CHANNELS < 8
+ packet[12] = 0x01;
+#else
+ packet[12] = 0x02;
+#endif
+ packet[13] = 0x00; //???
+ for(i = 8; i < 14; i++)
+ sum += packet[i];
+ packet[14] = sum >> 8;
+ packet[15] = sum & 0xff;
+}
+
+void build_data_packet(uint8_t upper)//
+{
+#if DSM2_NUM_CHANNELS==4
+ const uint8_t ch_map[] = {0, 1, 2, 3, 0xff, 0xff, 0xff}; //Guess
+#elif DSM2_NUM_CHANNELS==5
+ const uint8_t ch_map[] = {0, 1, 2, 3, 4, 0xff, 0xff}; //Guess
+#elif DSM2_NUM_CHANNELS==6
+ const uint8_t ch_map[] = {1, 5, 2, 3, 0, 4, 0xff}; //HP6DSM
+#elif DSM2_NUM_CHANNELS==7
+ const uint8_t ch_map[] = {1, 5, 2, 4, 3, 6, 0}; //DX6i
+#elif DSM2_NUM_CHANNELS==8
+ const uint8_t ch_map[] = {1, 5, 2, 3, 6, 0xff, 0xff, 4, 0, 7, 0xff, 0xff, 0xff, 0xff}; //DX8
+#elif DSM2_NUM_CHANNELS==9
+ const uint8_t ch_map[] = {3, 2, 1, 5, 0, 4, 6, 7, 8, 0xff, 0xff, 0xff, 0xff, 0xff}; //DM9
+#elif DSM2_NUM_CHANNELS==10
+ const uint8_t ch_map[] = {3, 2, 1, 5, 0, 4, 6, 7, 8, 9, 0xff, 0xff, 0xff, 0xff};
+#elif DSM2_NUM_CHANNELS==11
+ const uint8_t ch_map[] = {3, 2, 1, 5, 0, 4, 6, 7, 8, 9, 10, 0xff, 0xff, 0xff};
+#elif DSM2_NUM_CHANNELS==12
+ const uint8_t ch_map[] = {3, 2, 1, 5, 0, 4, 6, 7, 8, 9, 10, 11, 0xff, 0xff};
+#endif
+
+ uint8_t i;
+ uint8_t bits;
+ //
+ if( binding && PROTOCOL_SticksMoved(0) )
+ {
+ //BIND_DONE;
+ binding = 0;
+ }
+ if (sub_protocol==DSMX)
+ {
+ packet[0] = cyrfmfg_id[2];
+ packet[1] = cyrfmfg_id[3] + model;
+ bits=11;
+ }
+ else
+ {
+ packet[0] = (0xff ^ cyrfmfg_id[2]);
+ packet[1] = (0xff ^ cyrfmfg_id[3]) + model;
+ bits=10;
+ }
+ //
+ uint16_t max = 1 << bits;//max=2048 for DSMX & 1024 for DSM2 less than 8 ch and 2048 otherwise
+ //uint16_t pct_100 = (uint32_t)max * 100 / 150;//682 1024*100/150
+ //
+ for (i = 0; i < 7; i++)
+ {
+ uint8_t idx = ch_map[upper * 7 + i];//1,5,2,3,0,4
+ uint16_t value;
+ if (idx == 0xff)
+ value = 0xffff;
+ else
+ {
+ if (binding)
+ { // Failsafe position during binding
+ value=max/2; //all channels to middle
+ if(idx==0)
+ value=1; //except throttle
+ }
+ else
+ {
+ switch(idx)
+ {
+ case 0:
+ value=Servo_data[THROTTLE];//85.75-938.25=125%//171-853=100%
+ break;
+ case 1:
+ value=Servo_data[AILERON];
+ break;
+ case 2:
+ value=Servo_data[ELEVATOR];
+ break;
+ case 3:
+ value=Servo_data[RUDDER];
+ break;
+ case 4:
+ value=Servo_data[AUX1];
+ break;
+ case 5:
+ value=Servo_data[AUX2];
+ break;
+ case 6:
+ value=Servo_data[AUX3];
+ break;
+ case 7:
+ value=Servo_data[AUX4];
+ break;
+ }
+ value=map(value,PPM_MIN,PPM_MAX,0,max-1);
+ }
+ value |= (upper && i == 0 ? 0x8000 : 0) | (idx << bits);
+ }
+ packet[i*2+2] = (value >> 8) & 0xff;
+ packet[i*2+3] = (value >> 0) & 0xff;
+ }
+}
+
+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);//
+}
+
+uint8_t get_pn_row(uint8_t channel)
+{
+ return (sub_protocol == DSMX ? (channel - 2) % 5 : channel % 5);
+}
+
+const uint8_t init_vals[][2] = {
+ {CYRF_02_TX_CTRL, 0x00},
+ {CYRF_05_RX_CTRL, 0x00},
+ {CYRF_28_CLK_EN, 0x02},
+ {CYRF_32_AUTO_CAL_TIME, 0x3c},
+ {CYRF_35_AUTOCAL_OFFSET, 0x14},
+ {CYRF_06_RX_CFG, 0x4A},
+ {CYRF_1B_TX_OFFSET_LSB, 0x55},
+ {CYRF_1C_TX_OFFSET_MSB, 0x05},
+ {CYRF_0F_XACT_CFG, 0x24},
+ {CYRF_03_TX_CFG, 0x38 | CYRF_BIND_POWER},
+ {CYRF_12_DATA64_THOLD, 0x0a},
+ {CYRF_0F_XACT_CFG, 0x04},
+ {CYRF_39_ANALOG_CTRL, 0x01},
+ {CYRF_0F_XACT_CFG, 0x24}, //Force IDLE
+ {CYRF_29_RX_ABORT, 0x00}, //Clear RX abort
+ {CYRF_12_DATA64_THOLD, 0x0a}, //set pn correlation threshold
+ {CYRF_10_FRAMING_CFG, 0x4a}, //set sop len and threshold
+ {CYRF_29_RX_ABORT, 0x0f}, //Clear RX abort?
+ {CYRF_03_TX_CFG, 0x38 | CYRF_BIND_POWER}, //Set 64chip, SDE mode, was max-power but replaced by low power
+ {CYRF_10_FRAMING_CFG, 0x4a}, //set sop len and threshold
+ {CYRF_1F_TX_OVERRIDE, 0x04}, //disable tx CRC
+ {CYRF_1E_RX_OVERRIDE, 0x14}, //disable rx crc
+ {CYRF_14_EOP_CTRL, 0x02}, //set EOP sync == 2
+ {CYRF_01_TX_LENGTH, 0x10}, //16byte packet
+};
+
+void cyrf_config()
+{
+ for(uint8_t i = 0; i < sizeof(init_vals) / 2; i++)
+ CYRF_WriteRegister(init_vals[i][0], init_vals[i][1]);
+ CYRF_WritePreamble(0x333304);
+ CYRF_ConfigRFChannel(0x61);
+}
+
+void initialize_bind_state()
+{
+ const uint8_t pn_bind[] = { 0xc6,0x94,0x22,0xfe,0x48,0xe6,0x57,0x4e };
+ uint8_t data_code[32];
+ CYRF_ConfigRFChannel(BIND_CHANNEL); //This seems to be random?
+ uint8_t pn_row = get_pn_row(BIND_CHANNEL);
+ //printf("Ch: %d Row: %d SOP: %d Data: %d\n", BIND_CHANNEL, pn_row, sop_col, data_col);
+ CYRF_ConfigCRCSeed(crc);
+ CYRF_ConfigSOPCode(pncodes[pn_row][sop_col]);
+ memcpy(data_code, pncodes[pn_row][data_col], 16);
+ memcpy(data_code + 16, pncodes[0][8], 8);
+ memcpy(data_code + 24, pn_bind, 8);
+ CYRF_ConfigDataCode(data_code, 32);
+ build_bind_packet();
+}
+
+const uint8_t data_vals[][2] = {
+ {CYRF_05_RX_CTRL, 0x83}, //Initialize for reading RSSI
+ {CYRF_29_RX_ABORT, 0x20},
+ {CYRF_0F_XACT_CFG, 0x24},
+ {CYRF_29_RX_ABORT, 0x00},
+ {CYRF_03_TX_CFG, 0x08 | 7},
+ {CYRF_10_FRAMING_CFG, 0xea},
+ {CYRF_1F_TX_OVERRIDE, 0x00},
+ {CYRF_1E_RX_OVERRIDE, 0x00},
+ {CYRF_03_TX_CFG, 0x28 | 7},
+ {CYRF_12_DATA64_THOLD, 0x3f},
+ {CYRF_10_FRAMING_CFG, 0xff},
+ {CYRF_0F_XACT_CFG, 0x24}, //Switch from reading RSSI to Writing
+ {CYRF_29_RX_ABORT, 0x00},
+ {CYRF_12_DATA64_THOLD, 0x0a},
+ {CYRF_10_FRAMING_CFG, 0xea},
+};
+
+void cyrf_configdata()
+{
+ for(uint8_t i = 0; i < sizeof(data_vals) / 2; i++)
+ CYRF_WriteRegister(data_vals[i][0], data_vals[i][1]);
+}
+
+void 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);
+ CYRF_ConfigRFChannel(hopping_frequency[chidx]);
+ CYRF_ConfigCRCSeed(crcidx ? ~crc : crc);
+ CYRF_ConfigSOPCode(pncodes[pn_row][sop_col]);
+ CYRF_ConfigDataCode(pncodes[pn_row][data_col], 16);
+ if(sub_protocol == DSMX)
+ chidx = (chidx + 1) % 23;
+ else
+ chidx = (chidx + 1) % 2;
+ crcidx = !crcidx;
+}
+
+void 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));
+ uint32_t id_tmp = id;
+ while(idx < 23)
+ {
+ uint8_t i;
+ uint8_t count_3_27 = 0, count_28_51 = 0, count_52_76 = 0;
+ id_tmp = id_tmp * 0x0019660D + 0x3C6EF35F; // Randomization
+ uint8_t next_ch = ((id_tmp >> 8) % 0x49) + 3; // Use least-significant byte and must be larger than 3
+ if (((next_ch ^ id) & 0x01 )== 0)
+ continue;
+ for (i = 0; i < idx; i++)
+ {
+ if(hopping_frequency[i] == next_ch)
+ break;
+ if(hopping_frequency[i] <= 27)
+ count_3_27++;
+ else
+ if (hopping_frequency[i] <= 51)
+ count_28_51++;
+ else
+ count_52_76++;
+ }
+ if (i != idx)
+ continue;
+ if ((next_ch < 28 && count_3_27 < 8)
+ ||(next_ch >= 28 && next_ch < 52 && count_28_51 < 7)
+ ||(next_ch >= 52 && count_52_76 < 8))
+ hopping_frequency[idx++] = next_ch;
+ }
+}
+
+uint16_t ReadDsm2()
+{
+#define CH1_CH2_DELAY 4010 // Time between write of channel 1 and channel 2
+#define WRITE_DELAY 1650 // 1550 original, Time after write to verify write complete
+#define READ_DELAY 400 // Time before write to check read state, and switch channels
+ uint8_t i = 0;
+
+ switch(cyrf_state)
+ {
+ default:
+ //Binding
+ cyrf_state++;
+ if(cyrf_state & 1)
+ {
+ //Send packet on even states
+ //Note state has already incremented,
+ // so this is actually 'even' state
+ CYRF_WriteDataPacket(packet);
+ return 8500;
+ }
+ else
+ {
+ //Check status on odd states
+ CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS);
+ return 1500;
+ }
+ case DSM2_CHANSEL:
+ BIND_DONE;
+ //Select channels and configure for writing data
+ //CYRF_FindBestChannels(ch, 2, 10, 1, 79);
+ cyrf_configdata();
+ CYRF_SetTxRxMode(TX_EN);
+ chidx = 0;
+ crcidx = 0;
+ cyrf_state = DSM2_CH1_WRITE_A; // in fact cyrf_state++
+ set_sop_data_crc();
+ return 10000;
+ case DSM2_CH1_WRITE_A:
+ case DSM2_CH1_WRITE_B:
+ build_data_packet(cyrf_state == DSM2_CH1_WRITE_B);//compare state and DSM2_CH1_WRITE_B return 0 or 1
+ case DSM2_CH2_WRITE_A:
+ case DSM2_CH2_WRITE_B:
+ CYRF_WriteDataPacket(packet);
+ cyrf_state++; // change from WRITE to CHECK mode
+ return WRITE_DELAY;
+ case DSM2_CH1_CHECK_A:
+ case DSM2_CH1_CHECK_B:
+ while (! (CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02))
+ if(++i > NUM_WAIT_LOOPS)
+ break;
+ set_sop_data_crc();
+ cyrf_state++; // change from CH1_CHECK to CH2_WRITE
+ return CH1_CH2_DELAY - WRITE_DELAY;
+ case DSM2_CH2_CHECK_A:
+ case DSM2_CH2_CHECK_B:
+ while (! (CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02))
+ if(++i > NUM_WAIT_LOOPS)
+ break;
+ if (cyrf_state == DSM2_CH2_CHECK_A)
+ CYRF_SetPower(0x28); //Keep transmit power in sync
+ // No telemetry...
+ set_sop_data_crc();
+ if (cyrf_state == DSM2_CH2_CHECK_A)
+ {
+ #if DSM2_NUM_CHANNELS < 8
+ cyrf_state = DSM2_CH1_WRITE_A; // change from CH2_CHECK_A to CH1_WRITE_A (ie no upper)
+ return 11000 - CH1_CH2_DELAY - WRITE_DELAY ; // Original is 22000 from deviation but it works better this way
+ #else
+ cyrf_state = DSM2_CH1_WRITE_B; // change from CH2_CHECK_A to CH1_WRITE_A (to transmit upper)
+ #endif
+ }
+ else
+ cyrf_state = DSM2_CH1_WRITE_A; // change from CH2_CHECK_B to CH1_WRITE_A (upper already transmitted so transmit lower)
+ return 11000 - CH1_CH2_DELAY - WRITE_DELAY;
+ }
+ return 0;
+}
+
+uint16_t initDsm2()
+{
+ CYRF_Reset();
+ CYRF_GetMfgData(cyrfmfg_id);//
+
+ cyrf_config();
+
+ if (sub_protocol ==DSMX)
+ calc_dsmx_channel();
+ else
+ {
+#if RANDOM_CHANNELS == 1
+ uint8_t tmpch[10];
+ CYRF_FindBestChannels(tmpch, 10, 5, 3, 75);
+ //
+ randomSeed((uint32_t)analogRead(A6)<<10|analogRead(A7));//seed
+ uint8_t idx = random(0xfefefefe) % 10;
+ hopping_frequency[0] = tmpch[idx];
+ while(1)
+ {
+ idx = random(0xfefefefe) % 10;
+ if (tmpch[idx] != hopping_frequency[0])
+ break;
+ }
+ hopping_frequency[1] = tmpch[idx];
+#else
+ hopping_frequency[0] = (cyrfmfg_id[0] + cyrfmfg_id[2] + cyrfmfg_id[4]) % 39 + 1;
+ hopping_frequency[1] = (cyrfmfg_id[1] + cyrfmfg_id[3] + cyrfmfg_id[5]) % 40 + 40;
+#endif
+ }
+
+ ///}
+ crc = ~((cyrfmfg_id[0] << 8) + cyrfmfg_id[1]); //The crc for channel 'a' is NOT(mfgid[1] << 8 + mfgid[0])
+ crcidx = 0;//The crc for channel 'b' is (mfgid[1] << 8 + mfgid[0])
+ //
+ sop_col = (cyrfmfg_id[0] + cyrfmfg_id[1] + cyrfmfg_id[2] + 2) & 0x07;//Ok
+ data_col = 7 - sop_col;//ok
+
+ model=MProtocol_id-MProtocol_id_master; // RxNum for serial or 0 for ppm
+
+ CYRF_SetTxRxMode(TX_EN);
+ //
+ if(IS_AUTOBIND_FLAG_on)
+ {
+ cyrf_state = DSM2_BIND;
+ PROTOCOL_SticksMoved(1); //Initialize Stick position
+ initialize_bind_state();
+ binding = 1;
+ }
+ else
+ {
+ cyrf_state = DSM2_CHANSEL;//
+ binding = 0;
+ }
+ return 10000;
+}
+
+#endif
diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino
new file mode 100644
index 0000000..bfc08a8
--- /dev/null
+++ b/Multiprotocol/Devo_cyrf6936.ino
@@ -0,0 +1,391 @@
+/*
+ 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 .
+ */
+
+#if defined(DEVO_CYRF6936_INO)
+
+#include "iface_cyrf6936.h"
+
+#define DEVO_NUM_CHANNELS 8
+
+//For Debug
+//#define NO_SCRAMBLE
+#define PKTS_PER_CHANNEL 4
+#define DEVO_BIND_COUNT 0x1388
+//#define TELEMETRY_ENABLE 0x30
+#define NUM_WAIT_LOOPS (100 / 5) //each loop is ~5us. Do not wait more than 100us
+//
+//#define TELEM_ON 0
+//#define TELEM_OFF 1
+
+enum Devo_PhaseState
+{
+ DEVO_BIND,
+ DEVO_BIND_SENDCH,
+ DEVO_BOUND,
+ DEVO_BOUND_1,
+ DEVO_BOUND_2,
+ DEVO_BOUND_3,
+ DEVO_BOUND_4,
+ DEVO_BOUND_5,
+ DEVO_BOUND_6,
+ DEVO_BOUND_7,
+ DEVO_BOUND_8,
+ DEVO_BOUND_9,
+ DEVO_BOUND_10,
+};
+
+const uint8_t sopcodes[][8] = {
+ /* Note these are in order transmitted (LSB 1st) */
+ /* 0 */ {0x3C,0x37,0xCC,0x91,0xE2,0xF8,0xCC,0x91}, //0x91CCF8E291CC373C
+ /* 1 */ {0x9B,0xC5,0xA1,0x0F,0xAD,0x39,0xA2,0x0F}, //0x0FA239AD0FA1C59B
+ /* 2 */ {0xEF,0x64,0xB0,0x2A,0xD2,0x8F,0xB1,0x2A}, //0x2AB18FD22AB064EF
+ /* 3 */ {0x66,0xCD,0x7C,0x50,0xDD,0x26,0x7C,0x50}, //0x507C26DD507CCD66
+ /* 4 */ {0x5C,0xE1,0xF6,0x44,0xAD,0x16,0xF6,0x44}, //0x44F616AD44F6E15C
+ /* 5 */ {0x5A,0xCC,0xAE,0x46,0xB6,0x31,0xAE,0x46}, //0x46AE31B646AECC5A
+ /* 6 */ {0xA1,0x78,0xDC,0x3C,0x9E,0x82,0xDC,0x3C}, //0x3CDC829E3CDC78A1
+ /* 7 */ {0xB9,0x8E,0x19,0x74,0x6F,0x65,0x18,0x74}, //0x7418656F74198EB9
+ /* 8 */ {0xDF,0xB1,0xC0,0x49,0x62,0xDF,0xC1,0x49}, //0x49C1DF6249C0B1DF
+ /* 9 */ {0x97,0xE5,0x14,0x72,0x7F,0x1A,0x14,0x72}, //0x72141A7F7214E597
+};
+
+uint8_t txState;
+uint8_t pkt_num;
+uint8_t ch_idx;
+uint8_t use_fixed_id;
+uint8_t failsafe_pkt;
+
+void scramble_pkt()
+{
+#ifdef NO_SCRAMBLE
+ return;
+#else
+ uint8_t i;
+ for(i = 0; i < 15; i++)
+ packet[i + 1] ^= cyrfmfg_id[i % 4];
+#endif
+}
+
+void add_pkt_suffix()
+{
+ uint8_t bind_state;
+ if (use_fixed_id)
+ {
+ if (bind_counter > 0)
+ bind_state = 0xc0;
+ else
+ bind_state = 0x80;
+ }
+ else
+ bind_state = 0x00;
+ packet[10] = bind_state | (PKTS_PER_CHANNEL - pkt_num - 1);
+ packet[11] = *(hopping_frequency_ptr + 1);
+ packet[12] = *(hopping_frequency_ptr + 2);
+ packet[13] = fixed_id & 0xff;
+ packet[14] = (fixed_id >> 8) & 0xff;
+ packet[15] = (fixed_id >> 16) & 0xff;
+}
+
+void build_beacon_pkt(uint8_t upper)
+{
+ packet[0] = ((DEVO_NUM_CHANNELS << 4) | 0x07);
+// uint8_t enable = 0;
+ uint8_t max = 8;
+// int offset = 0;
+ if (upper)
+ {
+ packet[0] += 1;
+ max = 4;
+// offset = 8;
+ }
+ for(uint8_t i = 0; i < max; i++)
+ packet[i+1] = 0;
+// packet[9] = enable;
+ packet[9] = 0;
+ add_pkt_suffix();
+}
+
+void build_bind_pkt()
+{
+ packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x0a;
+ packet[1] = bind_counter & 0xff;
+ packet[2] = (bind_counter >> 8);
+ packet[3] = *hopping_frequency_ptr;
+ packet[4] = *(hopping_frequency_ptr + 1);
+ packet[5] = *(hopping_frequency_ptr + 2);
+ packet[6] = cyrfmfg_id[0];
+ packet[7] = cyrfmfg_id[1];
+ packet[8] = cyrfmfg_id[2];
+ packet[9] = cyrfmfg_id[3];
+ add_pkt_suffix();
+ //The fixed-id portion is scrambled in the bind packet
+ //I assume it is ignored
+ packet[13] ^= cyrfmfg_id[0];
+ packet[14] ^= cyrfmfg_id[1];
+ packet[15] ^= cyrfmfg_id[2];
+}
+
+void build_data_pkt()
+{
+ uint8_t i;
+ packet[0] = (DEVO_NUM_CHANNELS << 4) | (0x0b + ch_idx);
+ uint8_t sign = 0x0b;
+ for (i = 0; i < 4; i++)
+ {
+ //
+ int16_t value= map(Servo_data[ch_idx * 4 + i],PPM_MIN,PPM_MAX,-1600,1600);//range -1600...+1600
+ //s32 value = (s32)Channels[ch_idx * 4 + i] * 0x640 / CHAN_MAX_VALUE;//10000
+ if(value < 0)
+ {
+ value = -value;
+ sign |= 1 << (7 - i);
+ }
+ packet[2 * i + 1] = value & 0xff;
+ packet[2 * i + 2] = (value >> 8) & 0xff;
+ }
+ packet[9] = sign;
+ ch_idx = ch_idx + 1;
+ if (ch_idx * 4 >= DEVO_NUM_CHANNELS)
+ ch_idx = 0;
+ add_pkt_suffix();
+}
+
+void 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]);
+ if(! crc)
+ crc = 1;
+ uint8_t sopidx = (0xff &((cyrfmfg_id[0] << 2) + cyrfmfg_id[1] + cyrfmfg_id[2])) % 10;
+ CYRF_SetTxRxMode(TX_EN);
+ CYRF_ConfigCRCSeed((crc << 8) + crc);
+ CYRF_ConfigSOPCode(sopcodes[sopidx]);
+ CYRF_SetPower(0x08);
+}
+
+void cyrf_init()
+{
+ /* Initialise CYRF chip */
+ CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x39);
+ CYRF_SetPower(0x08);
+ CYRF_WriteRegister(CYRF_06_RX_CFG, 0x4A);
+ CYRF_WriteRegister(CYRF_0B_PWR_CTRL, 0x00);
+ CYRF_WriteRegister(CYRF_0D_IO_CFG, 0x04);
+ CYRF_WriteRegister(CYRF_0E_GPIO_CTRL, 0x20);
+ CYRF_WriteRegister(CYRF_10_FRAMING_CFG, 0xA4);
+ CYRF_WriteRegister(CYRF_11_DATA32_THOLD, 0x05);
+ CYRF_WriteRegister(CYRF_12_DATA64_THOLD, 0x0E);
+ CYRF_WriteRegister(CYRF_1B_TX_OFFSET_LSB, 0x55);
+ CYRF_WriteRegister(CYRF_1C_TX_OFFSET_MSB, 0x05);
+ CYRF_WriteRegister(CYRF_32_AUTO_CAL_TIME, 0x3C);
+ CYRF_WriteRegister(CYRF_35_AUTOCAL_OFFSET, 0x14);
+ CYRF_WriteRegister(CYRF_39_ANALOG_CTRL, 0x01);
+ CYRF_WriteRegister(CYRF_1E_RX_OVERRIDE, 0x10);
+ CYRF_WriteRegister(CYRF_1F_TX_OVERRIDE, 0x00);
+ CYRF_WriteRegister(CYRF_01_TX_LENGTH, 0x10);
+ CYRF_WriteRegister(CYRF_0C_XTAL_CTRL, 0xC0);
+ CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x10);
+ CYRF_WriteRegister(CYRF_27_CLK_OVERRIDE, 0x02);
+ CYRF_WriteRegister(CYRF_28_CLK_EN, 0x02);
+ CYRF_WriteRegister(CYRF_0F_XACT_CFG, 0x28);
+}
+
+void set_radio_channels()
+{
+ //int i;
+ CYRF_FindBestChannels(hopping_frequency, 3, 4, 4, 80);
+ //printf("Radio Channels:");
+ // for (i = 0; i < 3; i++) {
+ // printf(" %02x", radio_ch[i]);
+
+ //Serial.print(radio_ch[i]);
+ // }
+ // printf("\n");
+ //Makes code a little easier to duplicate these here
+ hopping_frequency[3] = hopping_frequency[0];
+ hopping_frequency[4] = hopping_frequency[1];
+}
+
+void DEVO_BuildPacket()
+{
+ switch(phase)
+ {
+ case DEVO_BIND:
+ if(bind_counter>0)
+ bind_counter--;
+ build_bind_pkt();
+ phase = DEVO_BIND_SENDCH;
+ break;
+ case DEVO_BIND_SENDCH:
+ if(bind_counter>0)
+ bind_counter--;
+ build_data_pkt();
+ scramble_pkt();
+ if (bind_counter == 0)
+ {
+ phase = DEVO_BOUND;
+ BIND_DONE;
+ }
+ else
+ phase = DEVO_BIND;
+ break;
+ case DEVO_BOUND:
+ case DEVO_BOUND_1:
+ case DEVO_BOUND_2:
+ case DEVO_BOUND_3:
+ case DEVO_BOUND_4:
+ case DEVO_BOUND_5:
+ case DEVO_BOUND_6:
+ case DEVO_BOUND_7:
+ case DEVO_BOUND_8:
+ case DEVO_BOUND_9:
+ build_data_pkt();
+ scramble_pkt();
+ phase++;
+ if (bind_counter > 0)
+ {
+ bind_counter--;
+ if (bind_counter == 0)
+ BIND_DONE;
+ }
+ break;
+ case DEVO_BOUND_10:
+ build_beacon_pkt(DEVO_NUM_CHANNELS > 8 ? failsafe_pkt : 0);
+ failsafe_pkt = failsafe_pkt ? 0 : 1;
+ scramble_pkt();
+ phase = DEVO_BOUND_1;
+ break;
+ }
+ pkt_num++;
+ if(pkt_num == PKTS_PER_CHANNEL)
+ pkt_num = 0;
+}
+
+uint16_t devo_callback()
+{
+ if (txState == 0)
+ {
+ txState = 1;
+ DEVO_BuildPacket();
+ CYRF_WriteDataPacket(packet);
+ return 1200;
+ }
+ txState = 0;
+ uint8_t i = 0;
+ while (! (CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02))
+ if(++i > NUM_WAIT_LOOPS)
+ return 1200;
+ if (phase == DEVO_BOUND)
+ {
+ /* exit binding state */
+ phase = DEVO_BOUND_3;
+ cyrf_set_bound_sop_code();
+ }
+ if(pkt_num == 0)
+ {
+ //Keep tx power updated
+ CYRF_SetPower(0x08);
+ hopping_frequency_ptr = hopping_frequency_ptr == &hopping_frequency[2] ? hopping_frequency : hopping_frequency_ptr + 1;
+ CYRF_ConfigRFChannel(*hopping_frequency_ptr);
+ }
+ return 1200;
+}
+
+void devo_bind()
+{
+ fixed_id = Model_fixed_id;
+ bind_counter = DEVO_BIND_COUNT;
+ use_fixed_id = 1;
+ //PROTOCOL_SetBindState(0x1388 * 2400 / 1000); //msecs 12000ms
+}
+
+/*
+void generate_fixed_id_bind(){
+if(BIND_0){
+//randomSeed((uint32_t)analogRead(A6)<<10|analogRead(A7));//seed
+uint8_t txid[4];
+//Model_fixed_id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
+Model_fixed_id=0x332211;
+txid[0]= (id &0xFF);
+txid[1] = ((id >> 8) & 0xFF);
+txid[2] = ((id >> 16) & 0xFF);
+//txid[3] = ((id >> 24) & 0xFF);
+eeprom_write_block((const void*)txid,(void*)40,3);
+devo_bind();
+}
+}
+*/
+
+
+uint16_t DevoInit()
+{
+ CYRF_Reset();
+ cyrf_init();
+ CYRF_GetMfgData(cyrfmfg_id);
+ CYRF_SetTxRxMode(TX_EN);
+ CYRF_ConfigCRCSeed(0x0000);
+ CYRF_ConfigSOPCode(sopcodes[0]);
+ set_radio_channels();
+ use_fixed_id = 0;
+ failsafe_pkt = 0;
+ hopping_frequency_ptr = hopping_frequency;
+ //
+ CYRF_ConfigRFChannel(*hopping_frequency_ptr);
+ //FIXME: Properly setnumber of channels;
+ pkt_num = 0;
+ ch_idx = 0;
+ txState = 0;
+ //uint8_t txid[4];
+ //
+
+ /*
+if(BIND_0){
+Model_fixed_id=0;
+eeprom_write_block((const void*)0,(void*)40,4);
+while(1){
+LED_ON;
+delay(100);
+LED_OFF;
+delay(100);
+}
+}
+else{
+eeprom_read_block((void*)txid,(const void*)40,3);
+Model_fixed_id=(txid[0] | ((uint32_t)txid[1]<<8) | ((uint32_t)txid[2]<<16));
+}
+*/
+
+ if(! Model_fixed_id)
+ {//model fixed ID =0
+ fixed_id = ((uint32_t)(hopping_frequency[0] ^ cyrfmfg_id[0] ^ cyrfmfg_id[3]) << 16)
+ | ((uint32_t)(hopping_frequency[1] ^ cyrfmfg_id[1] ^ cyrfmfg_id[4]) << 8)
+ | ((uint32_t)(hopping_frequency[2] ^ cyrfmfg_id[2] ^ cyrfmfg_id[5]) << 0);
+ fixed_id = fixed_id % 1000000;
+ bind_counter = DEVO_BIND_COUNT;
+ phase = DEVO_BIND;
+ //PROTOCOL_SetBindState(0x1388 * 2400 / 1000); //msecs
+ }
+ else
+ {
+ fixed_id = Model_fixed_id;
+ use_fixed_id = 1;
+ phase = DEVO_BOUND_1;
+ bind_counter = 0;
+ cyrf_set_bound_sop_code();
+ }
+
+ return 2400;
+}
+
+#endif
diff --git a/Multiprotocol/FlySky_a7105.ino b/Multiprotocol/FlySky_a7105.ino
new file mode 100644
index 0000000..eb3be83
--- /dev/null
+++ b/Multiprotocol/FlySky_a7105.ino
@@ -0,0 +1,210 @@
+/*
+ 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 .
+ */
+
+#if defined(FLYSKY_A7105_INO)
+
+#include "iface_a7105.h"
+
+//FlySky constants & variables
+#define FLYSKY_BIND_COUNT 2500
+
+const uint8_t PROGMEM tx_channels[] = {
+ 0x0a, 0x5a, 0x14, 0x64, 0x1e, 0x6e, 0x28, 0x78, 0x32, 0x82, 0x3c, 0x8c, 0x46, 0x96, 0x50, 0xa0,
+ 0xa0, 0x50, 0x96, 0x46, 0x8c, 0x3c, 0x82, 0x32, 0x78, 0x28, 0x6e, 0x1e, 0x64, 0x14, 0x5a, 0x0a,
+ 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x46, 0x96, 0x1e, 0x6e, 0x3c, 0x8c, 0x28, 0x78, 0x32, 0x82,
+ 0x82, 0x32, 0x78, 0x28, 0x8c, 0x3c, 0x6e, 0x1e, 0x96, 0x46, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a,
+ 0x28, 0x78, 0x0a, 0x5a, 0x50, 0xa0, 0x14, 0x64, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96,
+ 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x64, 0x14, 0xa0, 0x50, 0x5a, 0x0a, 0x78, 0x28,
+ 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x3c, 0x8c, 0x32, 0x82, 0x46, 0x96, 0x14, 0x64,
+ 0x64, 0x14, 0x96, 0x46, 0x82, 0x32, 0x8c, 0x3c, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50,
+ 0x50, 0xa0, 0x46, 0x96, 0x3c, 0x8c, 0x28, 0x78, 0x0a, 0x5a, 0x32, 0x82, 0x1e, 0x6e, 0x14, 0x64,
+ 0x64, 0x14, 0x6e, 0x1e, 0x82, 0x32, 0x5a, 0x0a, 0x78, 0x28, 0x8c, 0x3c, 0x96, 0x46, 0xa0, 0x50,
+ 0x46, 0x96, 0x3c, 0x8c, 0x50, 0xa0, 0x28, 0x78, 0x0a, 0x5a, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64,
+ 0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x5a, 0x0a, 0x78, 0x28, 0xa0, 0x50, 0x8c, 0x3c, 0x96, 0x46,
+ 0x46, 0x96, 0x0a, 0x5a, 0x3c, 0x8c, 0x14, 0x64, 0x50, 0xa0, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82,
+ 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0xa0, 0x50, 0x64, 0x14, 0x8c, 0x3c, 0x5a, 0x0a, 0x96, 0x46,
+ 0x46, 0x96, 0x0a, 0x5a, 0x50, 0xa0, 0x3c, 0x8c, 0x28, 0x78, 0x1e, 0x6e, 0x32, 0x82, 0x14, 0x64,
+ 0x64, 0x14, 0x82, 0x32, 0x6e, 0x1e, 0x78, 0x28, 0x8c, 0x3c, 0xa0, 0x50, 0x5a, 0x0a, 0x96, 0x46
+};
+
+enum {
+ // flags going to byte 10
+ FLAG_V9X9_VIDEO = 0x40,
+ FLAG_V9X9_CAMERA= 0x80,
+ // flags going to byte 12
+ FLAG_V9X9_UNK = 0x10, // undocumented ?
+ FLAG_V9X9_LED = 0x20,
+};
+
+enum {
+ // flags going to byte 13
+ FLAG_V6X6_HLESS1= 0x80,
+ // flags going to byte 14
+ FLAG_V6X6_VIDEO = 0x01,
+ FLAG_V6X6_YCAL = 0x02,
+ FLAG_V6X6_XCAL = 0x04,
+ FLAG_V6X6_RTH = 0x08,
+ FLAG_V6X6_CAMERA= 0x10,
+ FLAG_V6X6_HLESS2= 0x20,
+ FLAG_V6X6_LED = 0x40,
+ FLAG_V6X6_FLIP = 0x80,
+};
+
+enum {
+ // flags going to byte 14
+ FLAG_V912_TOPBTN= 0x40,
+ FLAG_V912_BTMBTN= 0x80,
+};
+
+uint8_t chanrow;
+uint8_t chancol;
+uint8_t chanoffset;
+
+void 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, };
+ static uint8_t seq_counter;
+ switch(sub_protocol) {
+ case V9X9:
+ if(Servo_data[AUX1] > PPM_SWITCH)
+ packet[12] |= FLAG_V9X9_UNK;
+ if(Servo_data[AUX2] > PPM_SWITCH)
+ packet[12] |= FLAG_V9X9_LED;
+ if(Servo_data[AUX3] > PPM_SWITCH)
+ packet[10] |= FLAG_V9X9_CAMERA;
+ if(Servo_data[AUX4] > PPM_SWITCH)
+ packet[10] |= FLAG_V9X9_VIDEO;
+ break;
+
+ case V6X6:
+ packet[13] = 0x03; // 3 = 100% rate (0=40%, 1=60%, 2=80%)
+ packet[14] = 0x00;
+ if(Servo_data[AUX1] > PPM_SWITCH)
+ packet[14] |= FLAG_V6X6_FLIP;
+ if(Servo_data[AUX2] > PPM_SWITCH)
+ packet[14] |= FLAG_V6X6_LED;
+ if(Servo_data[AUX3] > PPM_SWITCH)
+ packet[14] |= FLAG_V6X6_CAMERA;
+ if(Servo_data[AUX4] > PPM_SWITCH)
+ packet[14] |= FLAG_V6X6_VIDEO;
+ if(Servo_data[AUX5] > PPM_SWITCH)
+ {
+ packet[13] |= FLAG_V6X6_HLESS1;
+ packet[14] |= FLAG_V6X6_HLESS2;
+ }
+ if(Servo_data[AUX6] > PPM_SWITCH) //use option to manipulate these bytes
+ packet[14] |= FLAG_V6X6_RTH;
+ if(Servo_data[AUX7] > PPM_SWITCH)
+ packet[14] |= FLAG_V6X6_XCAL;
+ if(Servo_data[AUX8] > PPM_SWITCH)
+ packet[14] |= FLAG_V6X6_YCAL;
+ packet[15] = 0x10; // unknown
+ packet[16] = 0x10; // unknown
+ packet[17] = 0xAA; // unknown
+ packet[18] = 0xAA; // unknown
+ packet[19] = 0x60; // unknown, changes at irregular interval in stock TX
+ packet[20] = 0x02; // unknown
+ break;
+
+ case V912:
+ seq_counter++;
+ if( seq_counter > 9)
+ seq_counter = 0;
+ packet[12] |= 0x20; // bit 6 is always set ?
+ packet[13] = 0x00; // unknown
+ packet[14] = 0x00;
+ if(Servo_data[AUX1] > PPM_SWITCH)
+ packet[14] |= FLAG_V912_BTMBTN;
+ if(Servo_data[AUX2] > PPM_SWITCH)
+ packet[14] |= FLAG_V912_TOPBTN;
+ packet[15] = 0x27; // [15] and [16] apparently hold an analog channel with a value lower than 1000
+ packet[16] = 0x03; // maybe it's there for a pitch channel for a CP copter ?
+ packet[17] = V912_X17_SEQ[seq_counter]; // not sure what [17] & [18] are for
+ if(seq_counter == 0) // V912 Rx does not even read those bytes... [17-20]
+ packet[18] = 0x02;
+ else
+ packet[18] = 0x00;
+ packet[19] = 0x00; // unknown
+ packet[20] = 0x00; // unknown
+ break;
+
+ default:
+ break;
+ }
+}
+
+void flysky_build_packet(uint8_t init)
+{
+ uint8_t i;
+ //servodata timing range for flysky.
+ ////-100% =~ 0x03e8//=1000us(min)
+ //+100% =~ 0x07ca//=1994us(max)
+ //Center = 0x5d9//=1497us(center)
+ //channel order AIL;ELE;THR;RUD;AUX1;AUX2;AUX3;AUX4
+ packet[0] = init ? 0xaa : 0x55;
+ packet[1] = rx_tx_addr[3];
+ packet[2] = rx_tx_addr[2];
+ packet[3] = rx_tx_addr[1];
+ packet[4] = rx_tx_addr[0];
+ uint8_t ch[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4};
+ for(i = 0; i < 8; i++)
+ {
+ packet[5+2*i]=lowByte(Servo_data[ch[i]]); //low byte of servo timing(1000-2000us)
+ packet[6+2*i]=highByte(Servo_data[ch[i]]); //high byte of servo timing(1000-2000us)
+ }
+ flysky_apply_extension_flags();
+}
+
+uint16_t ReadFlySky()
+{
+ if (bind_counter)
+ {
+ flysky_build_packet(1);
+ A7105_WriteData(21, 1);
+ bind_counter--;
+ if (! bind_counter)
+ BIND_DONE;
+ }
+ else
+ {
+ flysky_build_packet(0);
+ A7105_WriteData(21, pgm_read_byte_near(&tx_channels[chanrow*16+chancol])-chanoffset);
+ chancol = (chancol + 1) % 16;
+ if (! chancol) //Keep transmit power updated
+ A7105_SetPower();
+ }
+ return 1460;
+}
+
+uint16_t initFlySky() {
+ //A7105_Reset();
+ A7105_Init(INIT_FLYSKY); //flysky_init();
+
+ if (rx_tx_addr[3] > 0x90) // limit offset to 9 as higher values don't work with some RX (ie V912)
+ rx_tx_addr[3] = rx_tx_addr[3] - 0x70;
+ chanrow=rx_tx_addr[3] % 16;
+ chancol=0;
+ chanoffset=rx_tx_addr[3] / 16;
+
+
+ if(IS_AUTOBIND_FLAG_on)
+ bind_counter = FLYSKY_BIND_COUNT;
+ else
+ bind_counter = 0;
+ return 2400;
+}
+
+#endif
diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino
new file mode 100644
index 0000000..11dfefe
--- /dev/null
+++ b/Multiprotocol/FrSkyX_cc2500.ino
@@ -0,0 +1,21 @@
+/*
+ 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 .
+ */
+
+#if defined(FRSKYX_CC2500_INO)
+
+#include "iface_cc2500.h"
+
+
+#endif
diff --git a/Multiprotocol/FrSky_cc2500.ino b/Multiprotocol/FrSky_cc2500.ino
new file mode 100644
index 0000000..44899fe
--- /dev/null
+++ b/Multiprotocol/FrSky_cc2500.ino
@@ -0,0 +1,269 @@
+/*
+ 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 .
+ */
+
+#if defined(FRSKY_CC2500_INO)
+
+#include "iface_cc2500.h"
+
+//##########Variables########
+uint32_t state;
+uint8_t len;
+
+enum {
+ FRSKY_BIND = 0,
+ FRSKY_BIND_DONE = 1000,
+ FRSKY_DATA1,
+ FRSKY_DATA2,
+ FRSKY_DATA3,
+ FRSKY_DATA4,
+ FRSKY_DATA5
+};
+
+uint16_t initFrSky_2way()
+{
+ if(IS_AUTOBIND_FLAG_on)
+ {
+ frsky2way_init(1);
+ state = FRSKY_BIND;//
+ }
+ else
+ {
+ frsky2way_init(0);
+ state = FRSKY_DATA2;
+ }
+ return 10000;
+}
+
+uint16_t ReadFrSky_2way()
+{
+ if (state < FRSKY_BIND_DONE)
+ {
+ 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_writeFifo(packet, packet[0]+1);
+ state++;
+ return 9000;
+ }
+ if (state == FRSKY_BIND_DONE)
+ {
+ state = FRSKY_DATA2;
+ frsky2way_init(0);
+ counter = 0;
+ BIND_DONE;
+ }
+ 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, get_chan_num(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;
+ if (len)//20 bytes
+ {
+ cc2500_readFifo(pkt, len); //received telemetry packets
+ #if defined(TELEMETRY)
+ //parse telemetry packet here
+ check_telemetry(pkt,len); //check if valid telemetry packets and buffer them.
+ #endif
+ }
+ CC2500_SetTxRxMode(TX_EN);
+ CC2500_SetPower(); // Set tx_power
+ }
+ cc2500_strobe(CC2500_SIDLE);
+ cc2500_writeReg(CC2500_0A_CHANNR, get_chan_num(counter % 47));
+ cc2500_writeReg(CC2500_23_FSCAL3, 0x89);
+ cc2500_strobe(CC2500_SFRX);
+ frsky2way_data_frame();
+ cc2500_writeFifo(packet, packet[0]+1);
+ state++;
+ }
+ return state == FRSKY_DATA4 ? 7500 : 9000;
+}
+
+#if defined(TELEMETRY)
+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=128){
+RSSI_dBm =(((uint16_t)(pktt[len-2])*18)>>5)- 82;
+}
+else{
+ RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>5)+65;
+ }
+}
+
+#endif
+
+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, fine);
+ //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########
+}
+
+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;
+}
+
+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;
+}
+
+uint8_t telemetry_counter=0;
+
+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] = pkt[6]?(telemetry_counter++)%32:0;
+ packet[5] = 0x01;
+ //
+ packet[10] = 0;
+ packet[11] = 0;
+ packet[16] = 0;
+ packet[17] = 0;
+ for(uint8_t i = 0; i < 8; i++)
+ {
+ uint16_t value;
+ value = convert_channel_frsky(i);
+ if(i < 4)
+ {
+ packet[6+i] = value & 0xff;
+ packet[10+(i>>1)] |= ((value >> 8) & 0x0f) << (4 *(i & 0x01));
+ }
+ else
+ {
+ packet[8+i] = value & 0xff;
+ packet[16+((i-4)>>1)] |= ((value >> 8) & 0x0f) << (4 * ((i-4) & 0x01));
+ }
+ }
+}
+
+#endif
diff --git a/Multiprotocol/Hisky_nrf24l01.ino b/Multiprotocol/Hisky_nrf24l01.ino
new file mode 100644
index 0000000..5555a81
--- /dev/null
+++ b/Multiprotocol/Hisky_nrf24l01.ino
@@ -0,0 +1,242 @@
+/*
+ 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 .
+ */
+
+#if defined(HISKY_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+#define BIND_COUNT 1000
+#define TXID_SIZE 5
+#define FREQUENCE_NUM 20
+//
+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]
+void calc_fh_channels(uint32_t seed)
+{
+ uint8_t idx = 0;
+ uint32_t rnd = seed;
+
+ while (idx < FREQUENCE_NUM)
+ {
+ uint8_t i;
+ uint8_t count_2_26 = 0, count_27_50 = 0, count_51_74 = 0;
+
+ rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization
+ // Use least-significant byte. 73 is prime, so channels 76..77 are unused
+ uint8_t next_ch = ((rnd >> 8) % 73) + 2;
+ // Keep the distance 2 between the channels - either odd or even
+ if (((next_ch ^ (uint8_t)seed) & 0x01 )== 0)
+ continue;
+ // Check that it's not duplicated and spread uniformly
+ for (i = 0; i < idx; i++) {
+ if(hopping_frequency[i] == next_ch)
+ break;
+ if(hopping_frequency[i] <= 26)
+ count_2_26++;
+ else if (hopping_frequency[i] <= 50)
+ count_27_50++;
+ else
+ count_51_74++;
+ }
+ if (i != idx)
+ continue;
+ if ( (next_ch <= 26 && count_2_26 < 8) || (next_ch >= 27 && next_ch <= 50 && count_27_50 < 8) || (next_ch >= 51 && count_51_74 < 8) )
+ hopping_frequency[idx++] = next_ch;//find hopping frequency
+ }
+}
+
+void build_binding_packet(void)
+{
+ uint8_t i;
+ uint16_t sum=0;
+ uint8_t sum_l,sum_h;
+
+ for(i=0;i<5;i++)
+ sum += rx_tx_addr[i];
+
+ sum_l = (uint8_t)sum;//low byte
+ sum >>= 8;
+ sum_h = (uint8_t)sum;//high bye
+
+ bind_buf_arry[0][0] = 0xff;
+ bind_buf_arry[0][1] = 0xaa;
+ bind_buf_arry[0][2] = 0x55;
+
+ for(i=3;i<8;i++)
+ bind_buf_arry[0][i] = rx_tx_addr[i-3];
+
+ for(i=1;i<4;i++)
+ {
+ bind_buf_arry[i][0] = sum_l;
+ bind_buf_arry[i][1] = sum_h;
+ bind_buf_arry[i][2] = i-1;
+ }
+
+ for(i=0;i<7;i++)
+ { bind_buf_arry[1][i+3] = hopping_frequency[i];
+ bind_buf_arry[2][i+3] = hopping_frequency[i+7];
+ bind_buf_arry[3][i+3] = hopping_frequency[i+14];
+ }
+}
+
+void hisky_init()
+{
+ NRF24L01_Initialize();
+
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00); // No Auto Acknowledgement
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01); // Enable p0 rx
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address (byte -2)
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, 81); // binding packet must be set in channel 81
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 10); // payload size = 10
+ if(sub_protocol==HK310)
+ NRF24L01_SetBitrate(NRF24L01_BR_250K); // 250Kbps
+ else
+ NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
+ NRF24L01_SetPower(); // Set power
+ NRF24L01_SetTxRxMode(TX_EN); // TX mode, 2-bytes CRC, radio on
+}
+
+// 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
+void build_ch_data()
+{
+ uint16_t temp;
+ uint8_t i,j;
+ uint8_t ch[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4};
+ for (i = 0; i< 8; i++) {
+ j=ch[i];
+ temp=map(limit_channel_100(j),PPM_MIN_100,PPM_MAX_100,0,1000);
+ if (j == THROTTLE) // It is clear that hisky's throttle stick is made reversely, so I adjust it here on purpose
+ temp = 1000 -temp;
+ if (j == AUX3)
+ temp = temp < 400 ? 0 : 3; // Gyro mode, 0 - 6 axis, 3 - 3 axis
+ packet[i] = (uint8_t)(temp&0xFF);
+ packet[i<4?8:9]>>=2;
+ packet[i<4?8:9]|=(temp>>2)&0xc0;
+ }
+}
+
+uint16_t hisky_cb()
+{
+ phase++;
+ if(sub_protocol==HK310)
+ switch(phase)
+ {
+ case 1:
+ NRF24L01_SetPower();
+ phase=2;
+ break;
+ case 4:
+ phase=6;
+ break;
+ case 7: // build packet and send failsafe every 100ms
+ convert_channel_HK310(hopping_frequency_no!=0?RUDDER:AUX2,&packet[0],&packet[1]);
+ convert_channel_HK310(hopping_frequency_no!=0?THROTTLE:AUX3,&packet[2],&packet[3]);
+ convert_channel_HK310(hopping_frequency_no!=0?AUX1:AUX4,&packet[4],&packet[5]);
+ packet[7]=hopping_frequency_no!=0?0x55:0xAA;
+ packet[8]=hopping_frequency_no!=0?0x67:0x5A;
+ phase=8;
+ break;
+ }
+ switch(phase)
+ {
+ case 1:
+ NRF24L01_FlushTx();
+ break;
+ case 2:
+ if (bind_counter != 0)
+ {
+ //Set TX id and channel for bind packet
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t *)"\x12\x23\x23\x45\x78", 5);
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, 81);
+ }
+ break;
+ case 3:
+ if (bind_counter != 0)
+ {
+ bind_counter--;//
+ if (! bind_counter) //Binding complete
+ BIND_DONE;//
+ //Send bind packet
+ NRF24L01_WritePayload(bind_buf_arry[binding_idx],10);
+ binding_idx++;
+ if (binding_idx >= 4)
+ binding_idx = 0;
+ }
+ break;
+ case 4:
+ if (bind_counter != 0)
+ NRF24L01_FlushTx();
+ break;
+ case 5:
+ //Set TX power
+ NRF24L01_SetPower();
+ break;
+ case 6:
+ //Set TX id and channel for normal packet
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no]);
+ hopping_frequency_no++;
+ if (hopping_frequency_no >= FREQUENCE_NUM)
+ hopping_frequency_no = 0;
+ break;
+ case 7:
+ //Build normal packet
+ build_ch_data();
+ break;
+ case 8:
+ break;
+ default:
+ //Send normal packet
+ phase = 0;
+ NRF24L01_WritePayload(packet,10);
+ break;
+ }
+ return 1000; // send 1 binding packet and 1 data packet per 9ms
+}
+
+// Generate internal id from TX id and manufacturer id (STM32 unique id)
+void initialize_tx_id()
+{
+ //Generate frequency hopping table
+ if(sub_protocol==HK310)
+ for(uint8_t i=0;i.
+ */
+
+#if defined(HUBSAN_A7105_INO)
+
+#include "iface_a7105.h"
+
+enum{
+ HUBSAN_FLAG_VIDEO = 0x01, // record video
+ HUBSAN_FLAG_FLIP = 0x08,
+ HUBSAN_FLAG_LIGHT = 0x04
+};
+
+uint32_t sessionid;
+const uint32_t txid = 0xdb042679;
+
+enum {
+ BIND_1,
+ BIND_2,
+ BIND_3,
+ BIND_4,
+ BIND_5,
+ BIND_6,
+ BIND_7,
+ BIND_8,
+ DATA_1,
+ DATA_2,
+ DATA_3,
+ DATA_4,
+ DATA_5,
+};
+#define WAIT_WRITE 0x80
+
+void update_crc()
+{
+ uint8_t sum = 0;
+ for(uint8_t i = 0; i < 15; i++)
+ sum += packet[i];
+ packet[15] = (256 - (sum % 256)) & 0xFF;
+}
+
+void hubsan_build_bind_packet(uint8_t state)
+{
+ packet[0] = state;
+ packet[1] = channel;
+ packet[2] = (sessionid >> 24) & 0xFF;
+ packet[3] = (sessionid >> 16) & 0xFF;
+ packet[4] = (sessionid >> 8) & 0xFF;
+ packet[5] = (sessionid >> 0) & 0xFF;
+ packet[6] = 0x08;
+ packet[7] = 0xe4;
+ packet[8] = 0xea;
+ packet[9] = 0x9e;
+ packet[10] = 0x50;
+ packet[11] = (txid >> 24) & 0xFF;
+ packet[12] = (txid >> 16) & 0xFF;
+ packet[13] = (txid >> 8) & 0xFF;
+ packet[14] = (txid >> 0) & 0xFF;
+ 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%
+void hubsan_build_packet()
+{
+ static uint8_t vtx_freq = 0;
+ memset(packet, 0, 16);
+ if(vtx_freq != option || packet_count==100) // set vTX frequency (H107D)
+ {
+ vtx_freq = option;
+ packet[0] = 0x40;
+ packet[1] = (option>0xF2)?0x17:0x16;
+ packet[2] = option+0x0D; // 5645 - 5900 MHz
+ packet[3] = 0x82;
+ packet_count++;
+ }
+ else //20 00 00 00 80 00 7d 00 84 02 64 db 04 26 79 7b
+ {
+ packet[0] = 0x20;
+ packet[2] = convert_channel_8b(THROTTLE);//throtle
+ }
+ packet[4] = 0xFF - convert_channel_8b(RUDDER);//Rudder is reversed
+ packet[6] = 0xFF - convert_channel_8b(ELEVATOR); //Elevator is reversed
+ packet[8] = convert_channel_8b(AILERON);//aileron
+ if( packet_count < 100) {
+ packet[9] = 0x02 | HUBSAN_FLAG_LIGHT | HUBSAN_FLAG_FLIP;
+ packet_count++;
+ }
+ else
+ {
+ packet[9] = 0x02;
+ // Channel 5
+ if( Servo_data[AUX1] >= PPM_SWITCH)
+ packet[9] |= HUBSAN_FLAG_FLIP;
+ // Channel 6
+ if( Servo_data[AUX2] >= PPM_SWITCH)
+ packet[9] |= HUBSAN_FLAG_LIGHT;
+ // Channel 8
+ if( Servo_data[AUX4] > PPM_SWITCH)
+ packet[9] |= HUBSAN_FLAG_VIDEO;
+ }
+ packet[10] = 0x64;
+ packet[11] = (txid >> 24) & 0xFF;
+ packet[12] = (txid >> 16) & 0xFF;
+ packet[13] = (txid >> 8) & 0xFF;
+ packet[14] = (txid >> 0) & 0xFF;
+ update_crc();
+}
+
+uint8_t hubsan_check_integrity()
+{
+ uint8_t sum = 0;
+ for(int i = 0; i < 15; i++)
+ sum += packet[i];
+ return packet[15] == ((256 - (sum % 256)) & 0xFF);
+}
+
+uint16_t ReadHubsan()
+{
+ static uint8_t txState=0;
+ static uint8_t rfMode=0;
+ uint16_t delay;
+ uint8_t i;
+
+ switch(phase) {
+ case BIND_1:
+ case BIND_3:
+ case BIND_5:
+ case BIND_7:
+ hubsan_build_bind_packet(phase == BIND_7 ? 9 : (phase == BIND_5 ? 1 : phase + 1 - BIND_1));
+ A7105_Strobe(A7105_STANDBY);
+ A7105_WriteData(16, channel);
+ phase |= WAIT_WRITE;
+ return 3000;
+ case BIND_1 | WAIT_WRITE:
+ case BIND_3 | WAIT_WRITE:
+ case BIND_5 | WAIT_WRITE:
+ case BIND_7 | WAIT_WRITE:
+ //wait for completion
+ for(i = 0; i< 20; i++) {
+ if(! (A7105_ReadReg(A7105_00_MODE) & 0x01))
+ break;
+ }
+ A7105_SetTxRxMode(RX_EN);
+ A7105_Strobe(A7105_RX);
+ phase &= ~WAIT_WRITE;
+ phase++;
+ return 4500; //7.5msec elapsed since last write
+ case BIND_2:
+ case BIND_4:
+ case BIND_6:
+ A7105_SetTxRxMode(TX_EN);
+ if(A7105_ReadReg(A7105_00_MODE) & 0x01) {
+ phase = BIND_1;
+ return 4500; //No signal, restart binding procedure. 12msec elapsed since last write
+ }
+ A7105_ReadData();
+ phase++;
+ if (phase == BIND_5)
+ A7105_WriteID(((uint32_t)packet[2] << 24) | ((uint32_t)packet[3] << 16) | ((uint32_t)packet[4] << 8) | packet[5]);
+ return 500; //8msec elapsed time since last write;
+ case BIND_8:
+ A7105_SetTxRxMode(TX_EN);
+ if(A7105_ReadReg(A7105_00_MODE) & 0x01) {
+ phase = BIND_7;
+ return 15000; //22.5msec elapsed since last write
+ }
+ A7105_ReadData();
+ if(packet[1] == 9) {
+ phase = DATA_1;
+ A7105_WriteReg(A7105_1F_CODE_I, 0x0F);
+ BIND_DONE;
+ return 28000; //35.5msec elapsed since last write
+ } else {
+ phase = BIND_7;
+ return 15000; //22.5 msec elapsed since last write
+ }
+ case DATA_1:
+ case DATA_2:
+ case DATA_3:
+ case DATA_4:
+ case DATA_5:
+ if( txState == 0) { // send packet
+ rfMode = A7105_TX;
+ if( phase == DATA_1)
+ A7105_SetPower(); //Keep transmit power in sync
+ hubsan_build_packet();
+ A7105_Strobe(A7105_STANDBY);
+ A7105_WriteData(16, phase == DATA_5 ? channel + 0x23 : channel);
+ if (phase == DATA_5)
+ phase = DATA_1;
+ else
+ phase++;
+ delay=3000;
+ }
+ else {
+#if defined(TELEMETRY)
+ if( rfMode == A7105_TX) {// switch to rx mode 3ms after packet sent
+ for( i=0; i<10; i++)
+ {
+ if( !(A7105_ReadReg(A7105_00_MODE) & 0x01)) {// wait for tx completion
+ A7105_SetTxRxMode(RX_EN);
+ A7105_Strobe(A7105_RX);
+ rfMode = A7105_RX;
+ break;
+ }
+ }
+ }
+ if( rfMode == A7105_RX) { // check for telemetry frame
+ for( i=0; i<10; i++) {
+ if( !(A7105_ReadReg(A7105_00_MODE) & 0x01)) { // data received
+ A7105_ReadData();
+ if( !(A7105_ReadReg(A7105_00_MODE) & 0x01)){ // data received
+ v_lipo=packet[13];// hubsan lipo voltage 8bits the real value is h_lipo/10(0x2A=42-4.2V)
+ telemetry_link=1;
+ }
+ A7105_Strobe(A7105_RX);
+ break;
+ }
+ }
+ }
+#endif
+ delay=1000;
+ }
+ if (++txState == 8) { // 3ms + 7*1ms
+ A7105_SetTxRxMode(TX_EN);
+ txState = 0;
+ }
+ return delay;
+ }
+ return 0;
+}
+
+uint16_t initHubsan() {
+ const uint8_t allowed_ch[] = {0x14, 0x1e, 0x28, 0x32, 0x3c, 0x46, 0x50, 0x5a, 0x64, 0x6e, 0x78, 0x82};
+ A7105_Init(INIT_HUBSAN); //hubsan_init();
+
+ randomSeed((uint32_t)analogRead(A0) << 10 | analogRead(A4));
+ sessionid = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
+ channel = allowed_ch[random(0xfefefefe) % sizeof(allowed_ch)];
+
+ BIND_IN_PROGRESS; // autobind protocol
+ phase = BIND_1;
+ packet_count=0;
+ return 10000;
+}
+
+#endif
diff --git a/Multiprotocol/KN_nrf24l01.ino b/Multiprotocol/KN_nrf24l01.ino
new file mode 100644
index 0000000..2a315e9
--- /dev/null
+++ b/Multiprotocol/KN_nrf24l01.ino
@@ -0,0 +1,274 @@
+/*
+ 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 .
+ */
+
+#if defined(KN_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+#define KN_BIND_COUNT 1000 // for KN 2sec every 2ms - 1000 packets
+// Timeout for callback in uSec, 2ms=2000us for KN
+#define KN_PACKET_PERIOD 2000
+#define KN_PACKET_CHKTIME 100 // Time to wait for packet to be sent (no ACK, so very short)
+
+//#define PAYLOADSIZE 16
+#define NFREQCHANNELS 4
+#define KN_TXID_SIZE 4
+
+
+enum {
+ KN_FLAG_DR = 0x01, // Dual Rate
+ KN_FLAG_TH = 0x02, // Throttle Hold
+ KN_FLAG_IDLEUP = 0x04, // Idle up
+ KN_FLAG_RES1 = 0x08,
+ KN_FLAG_RES2 = 0x10,
+ KN_FLAG_RES3 = 0x20,
+ KN_FLAG_GYRO3 = 0x40, // 00 - 6G mode, 01 - 3G mode
+ KN_FLAG_GYROR = 0x80 // Always 0 so far
+};
+
+//
+enum {
+ KN_INIT2 = 0,
+ KN_INIT2_NO_BIND,
+ KN_BIND,
+ KN_DATA
+};
+
+/*enum {
+ USE1MBPS_NO = 0,
+ USE1MBPS_YES = 1,
+};*/
+
+// 2-bytes CRC
+#define CRC_CONFIG (BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO))
+
+void kn_init()
+{
+ NRF24L01_Initialize();
+
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, CRC_CONFIG);
+ 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); // Activate feature register
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 1); // Dynamic payload for data pipe 0
+ // Enable: Dynamic Payload Length, Payload with ACK , W_TX_PAYLOAD_NOACK
+ NRF24L01_WriteReg(NRF24L01_1D_FEATURE, BV(NRF2401_1D_EN_DPL) | BV(NRF2401_1D_EN_ACK_PAY) | BV(NRF2401_1D_EN_DYN_ACK));
+ NRF24L01_Activate(0x73);
+}
+
+uint16_t kn_init2()
+{
+ NRF24L01_FlushTx();
+ NRF24L01_FlushRx();
+ packet_sent = 0;
+ packet_count = 0;
+ hopping_frequency_no = 0;
+
+ // Turn radio power on
+ NRF24L01_SetTxRxMode(TX_EN);
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, CRC_CONFIG | BV(NRF24L01_00_PWR_UP));
+ return 150;
+}
+
+void set_tx_for_bind()
+{
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, 83);
+ NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps for binding
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t *) "KNDZK", 5);
+}
+
+void set_tx_for_data()
+{
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
+}
+
+void kn_calc_fh_channels(uint32_t seed)
+{
+ uint8_t idx = 0;
+ uint32_t rnd = seed;
+ while (idx < NFREQCHANNELS) {
+ 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) % 73) + 2;
+ // Keep the distance 2 between the channels - either odd or even
+ if (((next_ch ^ seed) & 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;
+ }
+}
+
+void kn_initialize_tx_id()
+{
+ rx_tx_addr[4] = 'K';
+ kn_calc_fh_channels(MProtocol_id);
+}
+
+#define PACKET_COUNT_SHIFT 5
+#define RF_CHANNEL_SHIFT 2
+void kn_send_packet(uint8_t bind)
+{
+ uint8_t rf_ch;
+ if (bind) {
+ rf_ch = 83;
+ packet[0] = 'K';
+ packet[1] = 'N';
+ packet[2] = 'D';
+ packet[3] = 'Z';
+ packet[4] = rx_tx_addr[0];
+ packet[5] = rx_tx_addr[1];
+ packet[6] = rx_tx_addr[2];
+ packet[7] = rx_tx_addr[3];
+ packet[8] = hopping_frequency[0];
+ packet[9] = hopping_frequency[1];
+ packet[10] = hopping_frequency[2];
+ packet[11] = hopping_frequency[3];
+ packet[12] = 0x00;
+ packet[13] = 0x00;
+ packet[14] = 0x00;
+ packet[15] = 0x01; //mode_bitrate == USE1MBPS_YES ? 0x01 : 0x00;
+ } else {
+ rf_ch = hopping_frequency[hopping_frequency_no];
+
+ // Each packet is repeated 4 times on the same channel
+ // We're not strictly repeating them, rather we
+ // send new packet on the same frequency, so the
+ // receiver gets the freshest command. As receiver
+ // hops to a new frequency as soon as valid packet
+ // received it does not matter that the packet is
+ // not the same one repeated twice - nobody checks this
+
+ // NB! packet_count overflow is handled and used in
+ // callback.
+ if (++packet_count == 4)
+ hopping_frequency_no = (hopping_frequency_no + 1) & 0x03;
+
+ uint16_t kn_throttle, kn_rudder, kn_elevator, kn_aileron;
+ kn_throttle = convert_channel_10b(THROTTLE);
+ kn_aileron = convert_channel_10b(AILERON);
+ kn_elevator = convert_channel_10b(ELEVATOR);
+ kn_rudder = convert_channel_10b(RUDDER);
+ packet[0] = (kn_throttle >> 8) & 0xFF;
+ packet[1] = kn_throttle & 0xFF;
+ packet[2] = (kn_aileron >> 8) & 0xFF;
+ packet[3] = kn_aileron & 0xFF;
+ packet[4] = (kn_elevator >> 8) & 0xFF;
+ packet[5] = kn_elevator & 0xFF;
+ packet[6] = (kn_rudder >> 8) & 0xFF;
+ packet[7] = kn_rudder & 0xFF;
+ // Trims, middle is 0x64 (100) 0-200
+ packet[8] = 0x64; // T
+ packet[9] = 0x64; // A
+ packet[10] = 0x64; // E
+ packet[11] = 0x64; // R
+
+ if (Servo_data[AUX1] > PPM_SWITCH)
+ flags |= KN_FLAG_DR;
+ else
+ flags=0;
+ 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] = (packet_count << PACKET_COUNT_SHIFT) | (hopping_frequency_no << RF_CHANNEL_SHIFT);
+
+ packet[14] = 0x00;
+ packet[15] = 0x00;
+ }
+
+ packet_sent = 0;
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_ch);
+ NRF24L01_FlushTx();
+ NRF24L01_WritePayload(packet, 16);
+ //++total_packets;
+ packet_sent = 1;
+
+ if (! hopping_frequency_no) {
+ //Keep transmit power updated
+ NRF24L01_SetPower();
+ }
+}
+
+uint16_t kn_callback()
+{
+ uint16_t timeout = KN_PACKET_PERIOD;
+ switch (phase)
+ {
+ case KN_INIT2:
+ bind_counter = KN_BIND_COUNT;
+ timeout = kn_init2();
+ phase = KN_BIND;
+ set_tx_for_bind();
+ break;
+ case KN_INIT2_NO_BIND:
+ timeout = kn_init2();
+ phase = KN_DATA;
+ set_tx_for_data();
+ break;
+ case KN_BIND:
+ if (packet_sent && NRF24L01_packet_ack() != PKT_ACKED)
+ return KN_PACKET_CHKTIME;
+ kn_send_packet(1);
+ if (--bind_counter == 0) {
+ phase = KN_DATA;
+ set_tx_for_data();
+ BIND_DONE;
+ }
+ break;
+ case KN_DATA:
+ if (packet_count == 4)
+ packet_count = 0;
+ else {
+ if (packet_sent && NRF24L01_packet_ack() != PKT_ACKED)
+ return KN_PACKET_CHKTIME;
+ kn_send_packet(0);
+ }
+ break;
+ }
+ return timeout;
+}
+
+uint16_t initKN(){
+ //total_packets = 0;
+ //mode_bitrate=USE1MBPS_YES;
+ kn_init();
+ phase = IS_AUTOBIND_FLAG_on ? KN_INIT2 : KN_INIT2_NO_BIND;
+ kn_initialize_tx_id();
+
+ // Call callback in 50ms
+ return 50000;
+}
+
+#endif
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
new file mode 100644
index 0000000..1d1d866
--- /dev/null
+++ b/Multiprotocol/Multiprotocol.ino
@@ -0,0 +1,842 @@
+/*********************************************************
+ Multiprotocol Tx code
+ by Midelic and Pascal Langer(hpnuts)
+ http://www.rcgroups.com/forums/showthread.php?t=2165676
+ https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/edit/master/README.md
+
+ Thanks to PhracturedBlue
+ Ported from deviation firmware
+
+ 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 .
+*/
+#include
+#include
+#include
+
+#include "multiprotocol.h"
+
+//******************************************************
+// Multiprotocol module configuration starts here
+
+//Uncomment the TX type
+#define ER9X
+//#define DEVO7
+
+//Uncomment to enable 8 channels serial protocol, 16 otherwise
+//#define NUM_SERIAL_CH_8
+
+//Uncomment to enable telemetry
+#define TELEMETRY
+
+//Protocols to include in compilation, comment to exclude
+#define BAYANG_NRF24L01_INO
+#define CG023_NRF24L01_INO
+#define CX10_NRF24L01_INO
+#define DEVO_CYRF6936_INO
+#define DSM2_CYRF6936_INO
+#define FLYSKY_A7105_INO
+#define FRSKY_CC2500_INO
+#define HISKY_NRF24L01_INO
+#define HUBSAN_A7105_INO
+#define KN_NRF24L01_INO
+#define SLT_NRF24L01_INO
+#define SYMAX_NRF24L01_INO
+#define V2X2_NRF24L01_INO
+#define YD717_NRF24L01_INO
+//#define FRSKYX_CC2500_INO
+
+//Update this table to set which protocol/sub_protocol is called for the corresponding dial number
+static const uint8_t PPM_prot[15][2]= { {MODE_FLYSKY , Flysky }, //Dial=1
+ {MODE_HUBSAN , 0 }, //Dial=2
+ {MODE_FRSKY , 0 }, //Dial=3
+ {MODE_HISKY , Hisky }, //Dial=4
+ {MODE_V2X2 , 0 }, //Dial=5
+ {MODE_DSM2 , DSM2 }, //Dial=6
+ {MODE_DEVO , 0 }, //Dial=7
+ {MODE_YD717 , YD717 }, //Dial=8
+ {MODE_KN , 0 }, //Dial=9
+ {MODE_SYMAX , SYMAX }, //Dial=10
+ {MODE_SLT , 0 }, //Dial=11
+ {MODE_CX10 , CX10_BLUE }, //Dial=12
+ {MODE_CG023 , CG023 }, //Dial=13
+ {MODE_BAYANG , 0 }, //Dial=14
+ {MODE_SYMAX , SYMAX5C } //Dial=15
+ };
+
+//
+//TX definitions with timing endpoints and channels order
+//
+
+// Turnigy PPM and channels
+#if defined(ER9X)
+#define PPM_MAX 2140
+#define PPM_MIN 860
+#define PPM_MAX_100 2012
+#define PPM_MIN_100 988
+enum chan_order{
+ AILERON =0,
+ ELEVATOR,
+ THROTTLE,
+ RUDDER,
+ AUX1,
+ AUX2,
+ AUX3,
+ AUX4,
+ AUX5,
+ AUX6,
+ AUX7,
+ AUX8
+};
+#endif
+
+// Devo PPM and channels
+#if defined(DEVO7)
+#define PPM_MAX 2100
+#define PPM_MIN 900
+#define PPM_MAX_100 1920
+#define PPM_MIN_100 1120
+enum chan_order{
+ ELEVATOR=0,
+ AILERON,
+ THROTTLE,
+ RUDDER,
+ AUX1,
+ AUX2,
+ AUX3,
+ AUX4,
+ AUX5,
+ AUX6,
+ AUX7,
+ AUX8
+};
+#endif
+
+//CC2500 RF module frequency adjustment, use in case you cannot bind with Frsky RX
+//Note: this is set via Option when serial protocol is used
+//values from 0-127 offset increase frequency, values from 255 to 127 decrease base frequency
+//uint8_t fine = 0x00;
+uint8_t fine = 0xd7; //* 215=-41 *
+
+// Multiprotocol module configuration ends here
+//******************************************************
+
+
+//Global constants/variables
+
+uint32_t MProtocol_id;//tx id,
+uint32_t MProtocol_id_master;
+uint32_t Model_fixed_id=0;
+uint32_t fixed_id;
+uint8_t cyrfmfg_id[6];//for dsm2 and devo
+uint32_t blink=0;
+//
+uint16_t counter;
+uint8_t channel;
+uint8_t packet[40];
+
+#define NUM_CHN 16
+// Servo data
+uint16_t Servo_data[NUM_CHN];
+// PPM variable
+volatile uint16_t PPM_data[NUM_CHN];
+
+// NRF variables
+uint8_t rx_tx_addr[5];
+uint8_t phase;
+uint16_t bind_counter;
+uint8_t bind_phase;
+uint8_t binding_idx;
+uint32_t packet_counter;
+uint16_t packet_period;
+uint8_t packet_count;
+uint8_t packet_sent;
+uint8_t packet_length;
+uint8_t hopping_frequency[23];
+uint8_t *hopping_frequency_ptr;
+uint8_t hopping_frequency_no=0;
+uint8_t rf_ch_num;
+uint8_t throttle, rudder, elevator, aileron;
+uint8_t flags;
+
+// Mode_select variables
+uint8_t mode_select;
+uint8_t protocol_flags;
+
+// Serial variables
+#if defined(NUM_SERIAL_CH_8) //8 channels serial protocol
+#define RXBUFFER_SIZE 14
+#else //16 channels serial protocol
+#define RXBUFFER_SIZE 25
+#endif
+#define TXBUFFER_SIZE 12
+volatile uint8_t rx_buff[RXBUFFER_SIZE];
+volatile uint8_t rx_ok_buff[RXBUFFER_SIZE];
+volatile uint8_t tx_buff[TXBUFFER_SIZE];
+volatile uint8_t idx = 0;
+
+//Serial protocol
+uint8_t sub_protocol;
+uint8_t option;
+uint8_t cur_protocol[2];
+uint8_t prev_protocol=0;
+
+// Telemetry
+#if defined(TELEMETRY)
+uint8_t pkt[27];//telemetry receiving packets
+uint8_t pktt[27];//telemetry receiving packets
+volatile uint8_t tx_head;
+volatile uint8_t tx_tail;
+uint8_t v_lipo;
+int16_t RSSI_dBm;
+//const uint8_t RSSI_offset=72;//69 71.72 values db
+uint8_t telemetry_link=0;
+#include "telemetry.h"
+#endif
+
+// Callback
+typedef uint16_t (*void_function_t) (void);//pointer to a function with no parameters which return an uint16_t integer
+void_function_t remote_callback = 0;
+void CheckTimer(uint16_t (*cb)(void));
+
+// Init
+void setup()
+{
+ // General pinout
+ DDRD = (1<>2)&0x07 ) | ( (PINC<<3)&0x08) );//encoder dip switches 1,2,4,8=>B2,B3,B4,C0
+//**********************************
+//mode_select=14; // here to test PPM
+//**********************************
+
+ // Update LED
+ LED_OFF;
+ LED_SET_OUTPUT;
+
+ // Read or create protocol id
+ MProtocol_id=random_id(10,false);
+ MProtocol_id_master=MProtocol_id;
+
+ //Set power transmission flags
+ POWER_FLAG_on; //By default high power for everything
+
+ //Protocol and interrupts initialization
+ if(mode_select != MODE_SERIAL)
+ { // PPM
+ cur_protocol[0]= PPM_prot[mode_select-1][0];
+ sub_protocol = PPM_prot[mode_select-1][1];
+ protocol_init(cur_protocol[0]);
+
+ //Configure PPM interrupt
+ EICRA |=(1< led on
+ else
+ if(blink micros())
+ { // Callback did not took more than requested time for next callback
+ if(next_callback>32000)
+ { // next_callback should not be more than 32767 so we will wait here...
+ delayMicroseconds(next_callback-2000);
+ cli(); // disable global int
+ OCR1A=TCNT1+4000;
+ sei(); // enable global int
+ }
+ else
+ {
+ cli(); // disable global int
+ OCR1A+=next_callback*2; // set compare A for callback
+ sei(); // enable global int
+ }
+ TIFR1=(1<32000)
+ { // next_callback should not be more than 32767 so we will wait here...
+ delayMicroseconds(next_callback-2000);
+ next_callback=2000;
+ }
+ cli(); // disable global int
+ OCR1A=TCNT1+next_callback*2; // set compare A for callback
+ sei(); // enable global int
+ TIFR1=(1<>4)& 0x07; //subprotocol no (0-7) bits 4-6
+ MProtocol_id=MProtocol_id_master+(rx_ok_buff[1]& 0x0F); //personalized RX bind + rx num // rx_num bits 0---3
+ }
+ else
+ if( ((rx_ok_buff[0]&0x80)!=0) && ((cur_protocol[0]&0x80)==0) ) // Bind flag has been set
+ CHANGE_PROTOCOL_FLAG_on; //restart protocol with bind
+ cur_protocol[0] = rx_ok_buff[0]; //store current protocol
+
+// decode channel values
+#if defined(NUM_SERIAL_CH_8) //8 channels serial protocol
+ Servo_data[0]=rx_ok_buff[3]+((rx_ok_buff[11]&0xC0)<<2);
+ Servo_data[1]=rx_ok_buff[4]+((rx_ok_buff[11]&0x30)<<4);
+ Servo_data[2]=rx_ok_buff[5]+((rx_ok_buff[11]&0x0C)<<6);
+ Servo_data[3]=rx_ok_buff[6]+((rx_ok_buff[11]&0x03)<<8);
+ Servo_data[4]=rx_ok_buff[7]+((rx_ok_buff[12]&0xC0)<<2);
+ Servo_data[5]=rx_ok_buff[8]+((rx_ok_buff[12]&0x30)<<4);
+ Servo_data[6]=rx_ok_buff[9]+((rx_ok_buff[12]&0x0C)<<6);
+ Servo_data[7]=rx_ok_buff[10]+((rx_ok_buff[12]&0x03)<<8);
+ for(uint8_t i=0;i<8;i++)
+ Servo_data[i]=((Servo_data[i]*5)>>2)+860; //range 860-2140;
+#else //16 channels serial protocol
+ volatile uint8_t *p=rx_ok_buff+2;
+ uint8_t dec=-3;
+ for(uint8_t i=0;i=8)
+ {
+ dec-=8;
+ p++;
+ }
+ p++;
+ Servo_data[i]=((((*((uint32_t *)p))>>dec)&0x7FF)*5)/8+860; //value range 860<->2140 -125%<->+125%
+ }
+#endif
+ RX_FLAG_off; //data has been processed
+}
+
+void module_reset()
+{
+ remote_callback = 0;
+ switch(prev_protocol)
+ {
+ case MODE_FLYSKY:
+ case MODE_HUBSAN:
+ A7105_Reset();
+ break;
+ case MODE_FRSKY:
+ case MODE_FRSKYX:
+ CC2500_Reset();
+ break;
+ case MODE_HISKY:
+ case MODE_V2X2:
+ case MODE_YD717:
+ case MODE_KN:
+ case MODE_SYMAX:
+ case MODE_SLT:
+ case MODE_CX10:
+ NRF24L01_Reset();
+ break;
+ case MODE_DSM2:
+ case MODE_DEVO:
+ CYRF_Reset();
+ break;
+ }
+}
+
+// Channel value is converted to 8bit values full scale
+uint8_t convert_channel_8b(uint8_t num)
+{
+ return (uint8_t) (map(limit_channel_100(num),PPM_MIN_100,PPM_MAX_100,0,255));
+}
+
+// Channel value is converted to 8bit values to provided values scale
+uint8_t convert_channel_8b_scale(uint8_t num,uint8_t min,uint8_t max)
+{
+ return (uint8_t) (map(limit_channel_100(num),PPM_MIN_100,PPM_MAX_100,min,max));
+}
+
+// Channel value is converted sign + magnitude 8bit values
+uint8_t convert_channel_s8b(uint8_t num)
+{
+ uint8_t ch;
+ ch = convert_channel_8b(num);
+ return (ch < 128 ? 127-ch : ch);
+}
+
+// Channel value is converted to 10bit values
+uint16_t convert_channel_10b(uint8_t num)
+{
+ return (uint16_t) (map(limit_channel_100(num),PPM_MIN_100,PPM_MAX_100,1,1023));
+}
+
+// Channel value is multiplied by 1.5
+uint16_t convert_channel_frsky(uint8_t num)
+{
+ return Servo_data[num] + Servo_data[num]/2;
+}
+
+// Channel value is converted for HK310
+void convert_channel_HK310(uint8_t num, uint8_t *low, uint8_t *high)
+{
+ uint16_t temp=0xFFFF-(4*Servo_data[num])/3;
+ *low=(uint8_t)(temp&0xFF);
+ *high=(uint8_t)(temp>>8);
+}
+
+// Channel value is limited to PPM_100
+uint16_t limit_channel_100(uint8_t ch)
+{
+ if(Servo_data[ch]>PPM_MAX_100)
+ return PPM_MAX_100;
+ else
+ if (Servo_data[ch]> 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] = 0xC1; // for YD717: always uses first data port
+}
+
+#if defined(TELEMETRY)
+void Serial_write(uint8_t data)
+{
+ uint8_t t=tx_head;
+ if(++t>=TXBUFFER_SIZE)
+ t=0;
+ tx_buff[t]=data;
+ tx_head=t;
+ UCSR0B |= (1<
+ UBRR0H = UBRRH_VALUE;
+ UBRR0L = UBRRL_VALUE;
+ //Set frame format to 8 data bits, no parity, 1 stop bit
+ UCSR0C |= (1<
+ UBRR0H = UBRRH_VALUE;
+ UBRR0L = UBRRL_VALUE;
+ //Set frame format to 8 data bits, even parity, 2 stop bits
+ UCSR0C |= (1<> 8) & 0xFF);
+ txid[2] = ((id >> 16) & 0xFF);
+ txid[3] = ((id >> 24) & 0xFF);
+ eeprom_write_block((const void*)txid,(void*)adress,4);
+ eeprom_write_byte((uint8_t*)(adress+10),0xf0);//write bind flag in eeprom.
+ }
+ return id;
+}
+
+/**************************/
+/**************************/
+/** Interrupt routines **/
+/**************************/
+/**************************/
+
+ISR(INT1_vect)
+{ // Interrupt on PPM pin
+ static int8_t chan=-1;
+ static uint16_t Prev_TCNT1=0;
+ uint16_t Cur_TCNT1;
+
+ Cur_TCNT1=TCNT1-Prev_TCNT1; // Capture current Timer1 value
+ if(Cur_TCNT1<1000)
+ chan=-1; // bad frame
+ else
+ if(Cur_TCNT1>4840)
+ {
+ chan=0; // start of frame
+ PPM_FLAG_on; // full frame present (even at startup since PPM_data has been initialized)
+ }
+ else
+ if(chan!=-1) // need to wait for start of frame
+ { //servo values between 500us and 2420us will end up here
+ PPM_data[chan] = Cur_TCNT1/2;
+ if(PPM_data[chan]PPM_MAX) PPM_data[chan]=PPM_MAX;
+ if(chan++>=NUM_CHN)
+ chan=-1; // don't accept any new channels
+ }
+ Prev_TCNT1+=Cur_TCNT1;
+}
+
+#if defined(TELEMETRY)
+ISR(USART_UDRE_vect)
+{ // Transmit interrupt
+ uint8_t t = tx_tail;
+ if(tx_head!=t)
+ {
+ if(++t>=TXBUFFER_SIZE)//head
+ t=0;
+ UDR0=tx_buff[t];
+ tx_tail=t;
+ }
+ if (t == tx_head)
+ UCSR0B &= ~(1<>8) ^ rx_buff[idx++]) & 0xFF]);
+ }
+ else
+ { // A frame has been received and needs to be checked before giving it to main
+ TIMSK1 &=~(1<RXBUFFER_SIZE)
+ { // A full frame has been received
+ TIMSK1 &=~(1<.
+ */
+
+
+//---------------------------
+// AVR nrf chip bitbang SPI functions
+//---------------------------
+#include "iface_nrf24l01.h"
+
+void nrf_spi_write(uint8_t command)
+{
+ uint8_t n=8;
+
+ SCK_off;//SCK start low
+ SDI_off;
+ while(n--) {
+ if(command&0x80)
+ SDI_on;
+ else
+ SDI_off;
+ SCK_on;
+ NOP();
+ SCK_off;
+ command = command << 1;
+ }
+ SDI_on;
+}
+
+//VARIANT 2
+uint8_t nrf_spi_read(void)
+{
+ uint8_t result;
+ uint8_t i;
+ result=0;
+ for(i=0;i<8;i++) {
+ result<<=1;
+ if(SDO_1) ///
+ result|=0x01;
+ SCK_on;
+ NOP();
+ SCK_off;
+ NOP();
+ }
+ return result;
+}
+//--------------------------------------------
+
+
+
+//---------------------------
+// NRF24L01+ SPI Specific Functions
+//---------------------------
+
+uint8_t rf_setup;
+
+void NRF24L01_Initialize()
+{
+ rf_setup = 0x09;
+}
+
+void NRF24L01_WriteReg(uint8_t reg, uint8_t data)
+{
+ NRF_CSN_off;
+ nrf_spi_write(W_REGISTER | (REGISTER_MASK & reg));
+ nrf_spi_write(data);
+ NRF_CSN_on;
+}
+
+void NRF24L01_WriteRegisterMulti(uint8_t reg, uint8_t * data, uint8_t length)
+{
+ NRF_CSN_off;
+
+ nrf_spi_write(W_REGISTER | ( REGISTER_MASK & reg));
+ for (uint8_t i = 0; i < length; i++)
+ nrf_spi_write(data[i]);
+ NRF_CSN_on;
+}
+
+void NRF24L01_WritePayload(uint8_t * data, uint8_t length)
+{
+ NRF_CSN_off;
+ nrf_spi_write(W_TX_PAYLOAD);
+ for (uint8_t i = 0; i < length; i++)
+ nrf_spi_write(data[i]);
+ NRF_CSN_on;
+}
+
+uint8_t NRF24L01_ReadReg(uint8_t reg)
+{
+ NRF_CSN_off;
+ nrf_spi_write(R_REGISTER | (REGISTER_MASK & reg));
+ uint8_t data = nrf_spi_read();
+ NRF_CSN_on;
+ return data;
+}
+
+void NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t * data, uint8_t length)
+{
+ NRF_CSN_off;
+ nrf_spi_write(R_REGISTER | (REGISTER_MASK & reg));
+ for(uint8_t i = 0; i < length; i++)
+ data[i] = nrf_spi_read();
+ NRF_CSN_on;
+}
+
+void NRF24L01_ReadPayload(uint8_t * data, uint8_t length)
+{
+ NRF_CSN_off;
+ nrf_spi_write(R_RX_PAYLOAD);
+ for(uint8_t i = 0; i < length; i++)
+ data[i] = nrf_spi_read();
+ NRF_CSN_on;
+}
+
+void NRF24L01_Strobe(uint8_t state)
+{
+ NRF_CSN_off;
+ nrf_spi_write(state);
+ NRF_CSN_on;
+}
+
+void NRF24L01_FlushTx()
+{
+ NRF24L01_Strobe(FLUSH_TX);
+}
+
+void NRF24L01_FlushRx()
+{
+ NRF24L01_Strobe(FLUSH_RX);
+}
+
+void NRF24L01_Activate(uint8_t code)
+{
+ NRF_CSN_off;
+ nrf_spi_write(ACTIVATE);
+ nrf_spi_write(code);
+ NRF_CSN_on;
+}
+
+void NRF24L01_SetBitrate(uint8_t bitrate)
+{
+ // Note that bitrate 250kbps (and bit RF_DR_LOW) is valid only
+ // for nRF24L01+. There is no way to programmatically tell it from
+ // older version, nRF24L01, but the older is practically phased out
+ // by Nordic, so we assume that we deal with with modern version.
+
+ // Bit 0 goes to RF_DR_HIGH, bit 1 - to RF_DR_LOW
+ rf_setup = (rf_setup & 0xD7) | ((bitrate & 0x02) << 4) | ((bitrate & 0x01) << 3);
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, rf_setup);
+}
+
+void NRF24L01_SetPower_Value(uint8_t power)
+{
+ uint8_t nrf_power = 0;
+ switch(power) {
+ case TXPOWER_100uW: nrf_power = 0; break;
+ case TXPOWER_300uW: nrf_power = 0; break;
+ case TXPOWER_1mW: nrf_power = 0; break;
+ case TXPOWER_3mW: nrf_power = 1; break;
+ case TXPOWER_10mW: nrf_power = 1; break;
+ case TXPOWER_30mW: nrf_power = 2; break;
+ case TXPOWER_100mW: nrf_power = 3; break;
+ case TXPOWER_150mW: nrf_power = 3; break;
+ default: nrf_power = 0; break;
+ };
+ // Power is in range 0..3 for nRF24L01
+ rf_setup = (rf_setup & 0xF9) | ((nrf_power & 0x03) << 1);
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, rf_setup);
+}
+
+void NRF24L01_SetPower()
+{
+ uint8_t power=NRF_BIND_POWER;
+ if(IS_BIND_DONE_on)
+ power=IS_POWER_FLAG_on?NRF_HIGH_POWER:NRF_LOW_POWER;
+ else
+ if(IS_RANGE_FLAG_on)
+ power=NRF_POWER_0;
+ rf_setup = (rf_setup & 0xF9) | (power << 1);
+ NRF24L01_WriteReg(NRF24L01_06_RF_SETUP, rf_setup);
+}
+
+void NRF24L01_SetTxRxMode(enum TXRX_State mode)
+{
+ if(mode == TX_EN) {
+ NRF_CSN_off;
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, (1 << NRF24L01_07_RX_DR) //reset the flag(s)
+ | (1 << NRF24L01_07_TX_DS)
+ | (1 << NRF24L01_07_MAX_RT));
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, (1 << NRF24L01_00_EN_CRC) // switch to TX mode
+ | (1 << NRF24L01_00_CRCO)
+ | (1 << NRF24L01_00_PWR_UP));
+ _delay_us(130);
+ NRF_CSN_on;
+ }
+ else
+ if (mode == RX_EN) {
+ NRF_CSN_off;
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // reset the flag(s)
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0F); // switch to RX mode
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, (1 << NRF24L01_07_RX_DR) //reset the flag(s)
+ | (1 << NRF24L01_07_TX_DS)
+ | (1 << NRF24L01_07_MAX_RT));
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, (1 << NRF24L01_00_EN_CRC) // switch to RX mode
+ | (1 << NRF24L01_00_CRCO)
+ | (1 << NRF24L01_00_PWR_UP)
+ | (1 << NRF24L01_00_PRIM_RX));
+ _delay_us(130);
+ NRF_CSN_on;
+ }
+ else
+ {
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, (1 << NRF24L01_00_EN_CRC)); //PowerDown
+ NRF_CSN_off;
+ }
+}
+
+void NRF24L01_Reset()
+{
+ //** not in deviation but needed to hot switch between models
+ NRF24L01_Activate(0x73); // Activate feature register
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x00); // Disable dynamic payload length on all pipes
+ NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x00); // Set feature bits off
+ NRF24L01_Activate(0x73);
+ //**
+
+ NRF24L01_FlushTx();
+ NRF24L01_FlushRx();
+ NRF24L01_Strobe(0xff); // NOP
+ NRF24L01_ReadReg(0x07);
+ NRF24L01_SetTxRxMode(TXRX_OFF);
+ _delay_us(100);
+}
+
+uint8_t NRF24L01_packet_ack()
+{
+ switch (NRF24L01_ReadReg(NRF24L01_07_STATUS) & (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT))) {
+ case BV(NRF24L01_07_TX_DS):
+ return PKT_ACKED;
+ case BV(NRF24L01_07_MAX_RT):
+ return PKT_TIMEOUT;
+ }
+ return PKT_PENDING;
+}
+
+
+
+//---------------------------
+/*
+void NRF24L01_spi_test(void)
+{
+unsigned long errors = 0;
+unsigned long test = 0;
+unsigned long time;
+uint8_t test_data_r[5];
+uint8_t test_data_w[5] = {0x01,0x02,0x03,0x04,0x05};
+
+time = micros();
+Serial.println("Testing SPI");
+ for(test=0; test < 2775600 ; test++) // should run for X mins.
+ {
+ NRF24L01_WriteRegisterMulti(NRF24L01_0B_RX_ADDR_P1, test_data_w, 5);
+ NRF24L01_ReadRegisterMulti(NRF24L01_0B_RX_ADDR_P1, test_data_r, 5);
+ if(0 != memcmp(test_data_r, test_data_w, sizeof(test_data_r))) errors++;
+ test_data_w[0] ++;
+ test_data_w[1] ++;
+ test_data_w[2] ++;
+ test_data_w[3] ++;
+ test_data_w[4] ++;
+ }
+Serial.print("test "); Serial.print(test, HEX); Serial.print("\n");
+Serial.print("errors "); Serial.print(errors, HEX); Serial.print("\n");
+Serial.print("time "); Serial.print(micros()- time, DEC); Serial.print("\n");
+
+// 124211960
+// 90899216
+}
+*/
+//---------------------------
+
+
+///////////////
+// XN297 emulation layer
+uint8_t xn297_addr_len;
+uint8_t xn297_tx_addr[5];
+uint8_t xn297_rx_addr[5];
+uint8_t xn297_crc = 0;
+
+static const uint8_t xn297_scramble[] = {
+ 0xe3, 0xb1, 0x4b, 0xea, 0x85, 0xbc, 0xe5, 0x66,
+ 0x0d, 0xae, 0x8c, 0x88, 0x12, 0x69, 0xee, 0x1f,
+ 0xc7, 0x62, 0x97, 0xd5, 0x0b, 0x79, 0xca, 0xcc,
+ 0x1b, 0x5d, 0x19, 0x10, 0x24, 0xd3, 0xdc, 0x3f,
+ 0x8e, 0xc5, 0x2f};
+
+static const uint16_t xn297_crc_xorout[] = {
+ 0x0000, 0x3448, 0x9BA7, 0x8BBB, 0x85E1, 0x3E8C, // 1st entry is missing, probably never needed
+ 0x451E, 0x18E6, 0x6B24, 0xE7AB, 0x3828, 0x8148, // it's used for 3-byte address w/ 0 byte payload only
+ 0xD461, 0xF494, 0x2503, 0x691D, 0xFE8B, 0x9BA7,
+ 0x8B17, 0x2920, 0x8B5F, 0x61B1, 0xD391, 0x7401,
+ 0x2138, 0x129F, 0xB3A0, 0x2988};
+
+uint8_t bit_reverse(uint8_t b_in)
+{
+ uint8_t b_out = 0;
+ for (uint8_t i = 0; i < 8; ++i)
+ {
+ b_out = (b_out << 1) | (b_in & 1);
+ b_in >>= 1;
+ }
+ return b_out;
+}
+
+uint16_t crc16_update(uint16_t crc, uint8_t a)
+{
+ static const uint16_t polynomial = 0x1021;
+
+ crc ^= a << 8;
+ for (uint8_t i = 0; i < 8; ++i)
+ if (crc & 0x8000)
+ crc = (crc << 1) ^ polynomial;
+ else
+ crc = crc << 1;
+ return crc;
+}
+
+void XN297_SetTXAddr(const uint8_t* addr, uint8_t len)
+{
+ if (len > 5) len = 5;
+ if (len < 3) len = 3;
+ uint8_t buf[] = { 0x55, 0x0F, 0x71, 0x0C, 0x00 }; // bytes for XN297 preamble 0xC710F55 (28 bit)
+ xn297_addr_len = len;
+ if (xn297_addr_len < 4)
+ for (uint8_t i = 0; i < 4; ++i)
+ buf[i] = buf[i+1];
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, len-2);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, buf, 5);
+ // Receive address is complicated. We need to use scrambled actual address as a receive address
+ // but the TX code now assumes fixed 4-byte transmit address for preamble. We need to adjust it
+ // first. Also, if the scrambled address begins with 1 nRF24 will look for preamble byte 0xAA
+ // instead of 0x55 to ensure enough 0-1 transitions to tune the receiver. Still need to experiment
+ // with receiving signals.
+ memcpy(xn297_tx_addr, addr, len);
+}
+
+void XN297_SetRXAddr(const uint8_t* addr, uint8_t len)
+{
+ if (len > 5) len = 5;
+ if (len < 3) len = 3;
+ uint8_t buf[] = { 0, 0, 0, 0, 0 };
+ memcpy(buf, addr, len);
+ memcpy(xn297_rx_addr, addr, len);
+ for (uint8_t i = 0; i < xn297_addr_len; ++i)
+ buf[i] = xn297_rx_addr[i] ^ xn297_scramble[xn297_addr_len-i-1];
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, len-2);
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, buf, 5);
+}
+
+void XN297_Configure(uint8_t flags)
+{
+ xn297_crc = !!(flags & BV(NRF24L01_00_EN_CRC));
+ flags &= ~(BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO));
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, flags);
+}
+
+void XN297_WritePayload(uint8_t* msg, uint8_t len)
+{
+ uint8_t buf[32];
+ uint8_t last = 0;
+
+ if (xn297_addr_len < 4)
+ {
+ // If address length (which is defined by receive address length)
+ // is less than 4 the TX address can't fit the preamble, so the last
+ // byte goes here
+ buf[last++] = 0x55;
+ }
+ for (uint8_t i = 0; i < xn297_addr_len; ++i)
+ buf[last++] = xn297_tx_addr[xn297_addr_len-i-1] ^ xn297_scramble[i];
+
+ for (uint8_t i = 0; i < len; ++i) {
+ // bit-reverse bytes in packet
+ uint8_t b_out = bit_reverse(msg[i]);
+ buf[last++] = b_out ^ xn297_scramble[xn297_addr_len+i];
+ }
+ if (xn297_crc)
+ {
+ uint8_t offset = xn297_addr_len < 4 ? 1 : 0;
+ uint16_t crc = 0xb5d2;
+ for (uint8_t i = offset; i < last; ++i)
+ crc = crc16_update(crc, buf[i]);
+ crc ^= xn297_crc_xorout[xn297_addr_len - 3 + len];
+ buf[last++] = crc >> 8;
+ buf[last++] = crc & 0xff;
+ }
+ NRF24L01_WritePayload(buf, last);
+}
+
+void XN297_ReadPayload(uint8_t* msg, uint8_t len)
+{
+ NRF24L01_ReadPayload(msg, len);
+ for(uint8_t i=0; i.
+ */
+
+#if defined(SLT_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+// For code readability
+#define SLT_PAYLOADSIZE 7
+#define SLT_NFREQCHANNELS 15
+#define SLT_TXID_SIZE 4
+
+enum {
+ SLT_INIT2 = 0,
+ SLT_BIND,
+ SLT_DATA1,
+ SLT_DATA2,
+ SLT_DATA3
+};
+
+void SLT_init()
+{
+ NRF24L01_Initialize();
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO)); // 2-bytes CRC, radio off
+ 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, 0x02); // 4-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x00); // Disable auto retransmit
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 4); // bytes of data payload for pipe 1
+ NRF24L01_SetBitrate(NRF24L01_BR_250K); // 256kbps
+ NRF24L01_SetPower();
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, (uint8_t*)"\xC3\xC3\xAA\x55", 4);
+ NRF24L01_FlushRx();
+}
+
+static void SLT_init2()
+{
+ NRF24L01_FlushTx();
+ packet_sent = 0;
+ hopping_frequency_no = 0;
+
+ // Turn radio power on
+ NRF24L01_SetTxRxMode(TX_EN);
+}
+
+void SLT_set_tx_id(void)
+{
+ // Frequency hopping sequence generation
+ for (uint8_t i = 0; i < 4; ++i)
+ {
+ uint8_t next_i = (i+1) % 4; // is & 3 better than % 4 ?
+ uint8_t base = i < 2 ? 0x03 : 0x10;
+ hopping_frequency[i*4 + 0] = (rx_tx_addr[i] & 0x3f) + base;
+ hopping_frequency[i*4 + 1] = (rx_tx_addr[i] >> 2) + base;
+ hopping_frequency[i*4 + 2] = (rx_tx_addr[i] >> 4) + (rx_tx_addr[next_i] & 0x03)*0x10 + base;
+ if (i*4 + 3 < SLT_NFREQCHANNELS) // guard for 16 channel
+ hopping_frequency[i*4 + 3] = (rx_tx_addr[i] >> 6) + (rx_tx_addr[next_i] & 0x0f)*0x04 + base;
+ }
+
+ // unique
+ uint8_t done = 0;
+ for (uint8_t i = 0; i < SLT_NFREQCHANNELS; ++i)
+ while (!done)
+ {
+ done = 1;
+ for (uint8_t j = 0; j < i; ++j)
+ if (hopping_frequency[i] == hopping_frequency[j])
+ {
+ done = 0;
+ hopping_frequency[i] += 7;
+ if (hopping_frequency[i] >= 0x50)
+ hopping_frequency[i] = hopping_frequency[i] - 0x50 + 0x03;
+ }
+ }
+
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 4);
+}
+
+void wait_radio()
+{
+ if (packet_sent)
+ while (!(NRF24L01_ReadReg(NRF24L01_07_STATUS) & BV(NRF24L01_07_TX_DS))) ;
+ packet_sent = 0;
+}
+
+void send_data(uint8_t *data, uint8_t len)
+{
+ wait_radio();
+ NRF24L01_FlushTx();
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_RX_DR) | BV(NRF24L01_07_MAX_RT));
+ NRF24L01_WritePayload(data, len);
+ //NRF24L01_PulseCE();
+ packet_sent = 1;
+}
+
+void 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
+ uint8_t ch[]={AILERON, ELEVATOR, THROTTLE, RUDDER};
+ for (uint8_t i = 0; i < 4; ++i) {
+ uint16_t v = convert_channel_10b(ch[i]);
+ packet[i] = v;
+ e = (e >> 2) | (uint8_t) ((v >> 2) & 0xC0);
+ }
+ // Extra bits for AETR
+ packet[4] = e;
+ // 8-bit channels
+ packet[5] = convert_channel_8b(AUX1);
+ packet[6] = convert_channel_8b(AUX2);
+
+ // Set radio channel - once per packet batch
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no]);
+ if (++hopping_frequency_no >= SLT_NFREQCHANNELS)
+ hopping_frequency_no = 0;
+}
+
+static void send_bind_packet()
+{
+ wait_radio();
+
+ NRF24L01_SetPower();
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t *)"\x7E\xB8\x63\xA9", 4);
+
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x50);
+ send_data(rx_tx_addr, 4);
+
+ // NB: we should wait until the packet's sent before changing TX address!
+ wait_radio();
+
+ NRF24L01_SetPower();
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 4);
+}
+
+uint16_t SLT_callback()
+{
+ uint16_t delay_us = 20000; // 3 packets with 1ms intervals every 22ms
+ switch (phase)
+ {
+ case SLT_INIT2:
+ SLT_init2();
+ phase = SLT_BIND;
+ delay_us = 150;
+ break;
+ case SLT_BIND:
+ send_bind_packet();
+ phase = SLT_DATA1;
+ delay_us = 19000;
+ BIND_DONE;
+ break;
+ case SLT_DATA1:
+ SLT_build_packet();
+ send_data(packet, 7);
+ phase = SLT_DATA2;
+ delay_us = 1000;
+ break;
+ case SLT_DATA2:
+ send_data(packet, 7);
+ phase = SLT_DATA3;
+ delay_us = 1000;
+ break;
+ case SLT_DATA3:
+ send_data(packet, 7);
+ if (++counter >= 100)
+ {
+ counter = 0;
+ phase = SLT_BIND;
+ delay_us = 1000;
+ }
+ else
+ {
+ NRF24L01_SetPower(); // Set tx_power
+ phase = SLT_DATA1;
+ }
+ break;
+ }
+ return delay_us;
+}
+
+uint16_t initSLT()
+{
+ counter = 0;
+ SLT_init();
+ phase = SLT_INIT2;
+ SLT_set_tx_id();
+ BIND_IN_PROGRESS; // autobind protocol
+ return 50000;
+}
+
+#endif
diff --git a/Multiprotocol/Symax_nrf24l01.ino b/Multiprotocol/Symax_nrf24l01.ino
new file mode 100644
index 0000000..e6599b6
--- /dev/null
+++ b/Multiprotocol/Symax_nrf24l01.ino
@@ -0,0 +1,378 @@
+/*
+ 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 .
+ */
+
+#if defined(SYMAX_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+/***
+ Main protocol compatible with Syma X5C-1, X11, X11C, X12.
+ SymaX5C protocol option compatible with Syma X5C (original) and X2.
+***/
+
+#define SYMAX_BIND_COUNT 345 // 1.5 seconds
+#define SYMAX_FIRST_PACKET_DELAY 12000
+#define SYMAX_PACKET_PERIOD 4000 // Timeout for callback in uSec
+#define SYMAX_INITIAL_WAIT 500
+
+#define SYMAX_MAX_RF_CHANNELS 17
+
+#define SYMAX_FLAG_FLIP 0x01
+#define SYMAX_FLAG_VIDEO 0x02
+#define SYMAX_FLAG_PICTURE 0x04
+#define SYMAX_FLAG_HEADLESS 0x08
+
+#define SYMAX_PAYLOADSIZE 10 // receive data pipes set to this size, but unused
+#define SYMAX_MAX_PACKET_LENGTH 16 // X11,X12,X5C-1 10-byte, X5C 16-byte
+
+enum {
+ SYMAX_INIT1 = 0,
+ SYMAX_BIND2,
+ SYMAX_BIND3,
+ SYMAX_DATA
+};
+
+/*
+http://www.deviationtx.com/forum/protocol-development/3768-syma-x5c-1-x11-x12?start=140
+ TX address Channel Sequence
+S1 3B B6 00 00 A2 15 35 1D 3D
+D1 9A E9 02 00 A2 14 34 1C 3C
+
+D2 46 18 00 00 A2 11 21 31 41
+*/
+
+uint8_t SYMAX_checksum(uint8_t *data)
+{
+ uint8_t sum = data[0];
+
+ for (uint8_t i=1; i < packet_length-1; i++)
+ if ( sub_protocol==SYMAX5C )
+ sum += data[i];
+ else
+ sum ^= data[i];
+
+ return sum + ( sub_protocol==SYMAX5C ? 0 : 0x55 );
+}
+
+void SYMAX_read_controls()
+{
+ // Protocol is registered AETRF, that is
+ // Aileron is channel 1, Elevator - 2, Throttle - 3, Rudder - 4, Flip control - 5
+ aileron = convert_channel_s8b(AILERON);
+ elevator = convert_channel_s8b(ELEVATOR);
+ throttle = convert_channel_8b(THROTTLE);
+ rudder = convert_channel_s8b(RUDDER);
+
+ // Channel 5
+ if (Servo_data[AUX1] > PPM_SWITCH)
+ flags = SYMAX_FLAG_FLIP;
+ else
+ flags=0;
+ // Channel 7
+ if (Servo_data[AUX3] > PPM_SWITCH)
+ flags |= SYMAX_FLAG_PICTURE;
+ // Channel 8
+ if (Servo_data[AUX4] > PPM_SWITCH)
+ flags |= SYMAX_FLAG_VIDEO;
+ // Channel 9
+ if (Servo_data[AUX5] > PPM_SWITCH)
+ flags |= SYMAX_FLAG_HEADLESS;
+}
+
+#define X5C_CHAN2TRIM(X) ((((X) & 0x80 ? 0xff - (X) : 0x80 + (X)) >> 2) + 0x20)
+
+void SYMAX_build_packet_x5c(uint8_t bind)
+{
+ if (bind)
+ {
+ memset(packet, 0, packet_length);
+ packet[7] = 0xae;
+ packet[8] = 0xa9;
+ packet[14] = 0xc0;
+ packet[15] = 0x17;
+ }
+ else
+ {
+ SYMAX_read_controls();
+
+ packet[0] = throttle;
+ packet[1] = rudder;
+ packet[2] = elevator ^ 0x80; // reversed from default
+ packet[3] = aileron;
+ packet[4] = X5C_CHAN2TRIM(rudder ^ 0x80);// drive trims for extra control range
+ packet[5] = X5C_CHAN2TRIM(elevator);
+ packet[6] = X5C_CHAN2TRIM(aileron ^ 0x80);
+ packet[7] = 0xae;
+ packet[8] = 0xa9;
+ packet[9] = 0x00;
+ packet[10] = 0x00;
+ packet[11] = 0x00;
+ packet[12] = 0x00;
+ packet[13] = 0x00;
+ packet[14] = (flags & SYMAX_FLAG_VIDEO ? 0x10 : 0x00)
+ | (flags & SYMAX_FLAG_PICTURE ? 0x08 : 0x00)
+ | (flags & SYMAX_FLAG_FLIP ? 0x01 : 0x00)
+ | 0x04;// (flags & SYMAX_FLAG_RATES ? 0x04 : 0x00);
+ packet[15] = SYMAX_checksum(packet);
+ }
+}
+
+void SYMAX_build_packet(uint8_t bind)
+{
+ if (bind)
+ {
+ packet[0] = rx_tx_addr[4];
+ packet[1] = rx_tx_addr[3];
+ packet[2] = rx_tx_addr[2];
+ packet[3] = rx_tx_addr[1];
+ packet[4] = rx_tx_addr[0];
+ packet[5] = 0xaa;
+ packet[6] = 0xaa;
+ packet[7] = 0xaa;
+ packet[8] = 0x00;
+ }
+ else
+ {
+ SYMAX_read_controls();
+ packet[0] = throttle;
+ packet[1] = elevator;
+ packet[2] = rudder;
+ packet[3] = aileron;
+ packet[4] = (flags & SYMAX_FLAG_VIDEO ? 0x80 : 0x00) | (flags & SYMAX_FLAG_PICTURE ? 0x40 : 0x00);
+ packet[5] = (elevator >> 2) | 0xc0; //always high rates (bit 7 is rate control) (flags & SYMAX_FLAG_RATES ? 0x80 : 0x00) | 0x40; // use trims to extend controls
+ packet[6] = (rudder >> 2) | (flags & SYMAX_FLAG_FLIP ? 0x40 : 0x00);
+ packet[7] = (aileron >> 2) | (flags & SYMAX_FLAG_HEADLESS ? 0x80 : 0x00);
+ packet[8] = 0x00;
+ }
+ packet[9] = SYMAX_checksum(packet);
+}
+
+void SYMAX_send_packet(uint8_t bind)
+{
+ if (sub_protocol==SYMAX5C)
+ SYMAX_build_packet_x5c(bind);
+ else
+ SYMAX_build_packet(bind);
+
+ // clear packet status bits and TX FIFO
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x2e);
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no]);
+ NRF24L01_FlushTx();
+
+ NRF24L01_WritePayload(packet, packet_length);
+
+ if (packet_counter++ % 2) // use each channel twice
+ hopping_frequency_no = (hopping_frequency_no + 1) % rf_ch_num;
+
+ NRF24L01_SetPower(); // Set tx_power
+}
+
+static void symax_init()
+{
+ NRF24L01_Initialize();
+ //
+ NRF24L01_SetTxRxMode(TX_EN);
+ //
+ NRF24L01_ReadReg(NRF24L01_07_STATUS);
+ 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, 0x3F); // Enable all data pipes (even though not used?)
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xff); // 4mS retransmit t/o, 15 tries (retries w/o AA?)
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x08);
+
+ if (sub_protocol==SYMAX5C)
+ {
+ NRF24L01_SetBitrate(NRF24L01_BR_1M);
+ packet_length = 16;
+ }
+ else
+ {
+ NRF24L01_SetBitrate(NRF24L01_BR_250K);
+ packet_length = 10;
+ }
+ //
+ NRF24L01_SetPower();
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
+ NRF24L01_WriteReg(NRF24L01_08_OBSERVE_TX, 0x00);
+ NRF24L01_WriteReg(NRF24L01_09_CD, 0x00);
+ NRF24L01_WriteReg(NRF24L01_0C_RX_ADDR_P2, 0xC3); // LSB byte of pipe 2 receive address
+ NRF24L01_WriteReg(NRF24L01_0D_RX_ADDR_P3, 0xC4);
+ NRF24L01_WriteReg(NRF24L01_0E_RX_ADDR_P4, 0xC5);
+ NRF24L01_WriteReg(NRF24L01_0F_RX_ADDR_P5, 0xC6);
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, SYMAX_PAYLOADSIZE); // bytes of data payload for pipe 1
+ NRF24L01_WriteReg(NRF24L01_12_RX_PW_P1, SYMAX_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_13_RX_PW_P2, SYMAX_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_14_RX_PW_P3, SYMAX_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_15_RX_PW_P4, SYMAX_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_16_RX_PW_P5, SYMAX_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
+
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR , sub_protocol==SYMAX5C ? (uint8_t *)"\x6D\x6A\x73\x73\x73" : (uint8_t *)"\xAB\xAC\xAD\xAE\xAF" ,5);
+
+ NRF24L01_ReadReg(NRF24L01_07_STATUS);
+ NRF24L01_FlushTx();
+ NRF24L01_ReadReg(NRF24L01_07_STATUS);
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x0e);
+ NRF24L01_ReadReg(NRF24L01_00_CONFIG);
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0c);
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0e); // power on
+}
+
+void 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};
+ uint8_t chans_bind[] = {0x4b, 0x30, 0x40, 0x20};
+ uint8_t chans_bind_x5c[] = {0x27, 0x1b, 0x39, 0x28, 0x24, 0x22, 0x2e, 0x36,
+ 0x19, 0x21, 0x29, 0x14, 0x1e, 0x12, 0x2d, 0x18};
+
+ //uint8_t data_rx_tx_addr[] = {0x3b,0xb6,0x00,0x00,0xa2};
+ //uint8_t data_rx_tx_addr[] = {0x9A,0xe9,0x03,0x00,0xa2};//<<---- is ok
+ //uint8_t data_rx_tx_addr[] = {0x3b,0xb6,0x00,0x00,0xa2};//<<--- is ok
+ //uint8_t data_rx_tx_addr[] = {0x9A,0xe9,0x00,0x00,0xa2};
+ //uint8_t data_rx_tx_addr[] = {0x9A,0xe9,0x03,0x00,0xa2};//<<---- is ok
+ //uint8_t data_rx_tx_addr[] = {0x46,0x18,0x00,0x00,0xa2};
+
+ NRF24L01_FlushTx();
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x08);
+ NRF24L01_WritePayload(first_packet, 15);
+
+ if (sub_protocol==SYMAX5C)
+ {
+ rf_ch_num = sizeof(chans_bind_x5c);
+ memcpy(hopping_frequency, chans_bind_x5c, rf_ch_num);
+ }
+ else
+ {
+ rx_tx_addr[4] = 0xa2; // this is constant in ID
+ rf_ch_num = sizeof(chans_bind);
+ memcpy(hopping_frequency, chans_bind, rf_ch_num);
+ }
+ hopping_frequency_no = 0;
+ packet_counter = 0;
+}
+
+// channels determined by last byte of tx address
+void 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};
+ static const uint8_t start_chans_3[] = {0x1a, 0x3a, 0x12, 0x32};
+ //static const uint8_t start_chans_4[] = {0x15, 0x35, 0x1d, 0x3d};
+ //static const uint8_t start_chans_5[] = {0x14, 0x34, 0x1c, 0x3c};
+ //static const uint8_t start_chans_6[] = {0x11, 0x21, 0x31, 0x41};
+ uint8_t laddress = address & 0x1f;
+ uint8_t i;
+ uint32_t *pchans = (uint32_t *)hopping_frequency; // avoid compiler warning
+
+ rf_ch_num = 4;
+
+ if (laddress < 0x10)
+ {
+ if (laddress == 6)
+ laddress = 7;
+ for(i=0; i < rf_ch_num; i++)
+ hopping_frequency[i] = start_chans_1[i] + laddress;
+ }
+ else
+ if (laddress < 0x18)
+ {
+ for(i=0; i < rf_ch_num; i++)
+ hopping_frequency[i] = start_chans_2[i] + (laddress & 0x07);
+ if (laddress == 0x16)
+ {
+ hopping_frequency[0]++;
+ hopping_frequency[1]++;
+ }
+ }
+ else
+ if (laddress < 0x1e)
+ {
+ for(i=0; i < rf_ch_num; i++)
+ hopping_frequency[i] = start_chans_3[i] + (laddress & 0x07);
+ }
+ else
+ if (laddress == 0x1e)
+ *pchans = 0x38184121;
+ else
+ *pchans = 0x39194121;
+}
+
+void symax_init2()
+{
+ uint8_t chans_data_x5c[] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24,
+ 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22};
+
+ if (sub_protocol==SYMAX5C)
+ {
+ rf_ch_num = sizeof(chans_data_x5c);
+ memcpy(hopping_frequency, chans_data_x5c, rf_ch_num);
+ }
+ else
+ {
+ symax_set_channels(rx_tx_addr[0]);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
+ }
+ hopping_frequency_no = 0;
+ packet_counter = 0;
+}
+
+uint16_t symax_callback()
+{
+ switch (phase)
+ {
+ case SYMAX_INIT1:
+ symax_init1();
+ phase = SYMAX_BIND2;
+ return SYMAX_FIRST_PACKET_DELAY;
+ break;
+ case SYMAX_BIND2:
+ counter = SYMAX_BIND_COUNT;
+ phase = SYMAX_BIND3;
+ SYMAX_send_packet(1);
+ break;
+ case SYMAX_BIND3:
+ if (counter == 0)
+ {
+ symax_init2();
+ phase = SYMAX_DATA;
+ BIND_DONE;
+ }
+ else
+ {
+ SYMAX_send_packet(1);
+ counter--;
+ }
+ break;
+ case SYMAX_DATA:
+ SYMAX_send_packet(0);
+ break;
+ }
+ return SYMAX_PACKET_PERIOD;
+}
+
+uint16_t initSymax()
+{
+ packet_counter = 0;
+ flags = 0;
+ BIND_IN_PROGRESS; // autobind protocol
+ symax_init();
+ phase = SYMAX_INIT1;
+ return SYMAX_INITIAL_WAIT;
+}
+
+#endif
diff --git a/Multiprotocol/V2X2_nrf24l01.ino b/Multiprotocol/V2X2_nrf24l01.ino
new file mode 100644
index 0000000..fed869d
--- /dev/null
+++ b/Multiprotocol/V2X2_nrf24l01.ino
@@ -0,0 +1,274 @@
+/*
+ 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 .
+ */
+
+#if defined(V2X2_NRF24L01_INO)
+
+// compatible with WLToys V2x2, JXD JD38x, JD39x, JJRC H6C, Yizhan Tarantula X6 ...
+
+#include "iface_nrf24l01.h"
+
+
+#define BIND_COUNT 1000
+// Timeout for callback in uSec, 4ms=4000us for V202
+#define PACKET_PERIOD 4000
+//
+// Time to wait for packet to be sent (no ACK, so very short)
+#define PACKET_CHKTIME 100
+
+//
+enum {
+ V2X2_FLAG_CAMERA = 0x01, // also automatic Missile Launcher and Hoist in one direction
+ V2X2_FLAG_VIDEO = 0x02, // also Sprayer, Bubbler, Missile Launcher(1), and Hoist in the other dir.
+ V2X2_FLAG_FLIP = 0x04,
+ V2X2_FLAG_UNK9 = 0x08,
+ V2X2_FLAG_LIGHT = 0x10,
+ V2X2_FLAG_UNK10 = 0x20,
+ V2X2_FLAG_BIND = 0xC0,
+ // flags going to byte 10
+ V2X2_FLAG_HEADLESS = 0x02,
+ V2X2_FLAG_MAG_CAL_X = 0x08,
+ V2X2_FLAG_MAG_CAL_Y = 0x20
+};
+
+//
+#define V2X2_PAYLOADSIZE 16
+
+enum {
+ V202_INIT2 = 0,
+ V202_INIT2_NO_BIND,//1
+ V202_BIND1,//2
+ V202_BIND2,//3
+ V202_DATA//4
+};
+
+// static u32 bind_count;
+
+// This is frequency hopping table for V202 protocol
+// The table is the first 4 rows of 32 frequency hopping
+// patterns, all other rows are derived from the first 4.
+// For some reason the protocol avoids channels, dividing
+// by 16 and replaces them by subtracting 3 from the channel
+// number in this case.
+// The pattern is defined by 5 least significant bits of
+// sum of 3 bytes comprising TX id
+static const uint8_t freq_hopping[][16] = {
+ { 0x27, 0x1B, 0x39, 0x28, 0x24, 0x22, 0x2E, 0x36,
+ 0x19, 0x21, 0x29, 0x14, 0x1E, 0x12, 0x2D, 0x18 }, // 00
+ { 0x2E, 0x33, 0x25, 0x38, 0x19, 0x12, 0x18, 0x16,
+ 0x2A, 0x1C, 0x1F, 0x37, 0x2F, 0x23, 0x34, 0x10 }, // 01
+ { 0x11, 0x1A, 0x35, 0x24, 0x28, 0x18, 0x25, 0x2A,
+ 0x32, 0x2C, 0x14, 0x27, 0x36, 0x34, 0x1C, 0x17 }, // 02
+ { 0x22, 0x27, 0x17, 0x39, 0x34, 0x28, 0x2B, 0x1D,
+ 0x18, 0x2A, 0x21, 0x38, 0x10, 0x26, 0x20, 0x1F } // 03
+};
+//static uint8_t hopping_frequency[16];
+
+void v202_init()
+{
+ NRF24L01_Initialize();
+
+ // 2-bytes CRC, radio off
+ 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, 0x3F); // Enable all data pipes
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0xFF); // 4ms retransmit t/o, 15 tries
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, 0x08); // Channel 8
+ NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
+ NRF24L01_SetPower();
+
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
+ // NRF24L01_WriteReg(NRF24L01_08_OBSERVE_TX, 0x00); // no write bits in this field
+ // NRF24L01_WriteReg(NRF24L01_00_CD, 0x00); // same
+ NRF24L01_WriteReg(NRF24L01_0C_RX_ADDR_P2, 0xC3); // LSB byte of pipe 2 receive address
+ NRF24L01_WriteReg(NRF24L01_0D_RX_ADDR_P3, 0xC4);
+ NRF24L01_WriteReg(NRF24L01_0E_RX_ADDR_P4, 0xC5);
+ NRF24L01_WriteReg(NRF24L01_0F_RX_ADDR_P5, 0xC6);
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, V2X2_PAYLOADSIZE); // bytes of data payload for pipe 1
+ NRF24L01_WriteReg(NRF24L01_12_RX_PW_P1, V2X2_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_13_RX_PW_P2, V2X2_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_14_RX_PW_P3, V2X2_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_15_RX_PW_P4, V2X2_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_16_RX_PW_P5, V2X2_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
+ uint8_t v2x2_rx_tx_addr[] = {0x66, 0x88, 0x68, 0x68, 0x68};
+ uint8_t rx_p1_addr[] = {0x88, 0x66, 0x86, 0x86, 0x86};
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, v2x2_rx_tx_addr, 5);
+ NRF24L01_WriteRegisterMulti(NRF24L01_0B_RX_ADDR_P1, rx_p1_addr, 5);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, v2x2_rx_tx_addr, 5);
+}
+
+void V202_init2()
+{
+ NRF24L01_FlushTx();
+ packet_sent = 0;
+ hopping_frequency_no = 0;
+
+ // Turn radio power on
+ NRF24L01_SetTxRxMode(TX_EN);
+ //Done by TX_EN??? => NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO) | BV(NRF24L01_00_PWR_UP));
+}
+
+void set_tx_id(void)
+{
+ uint8_t sum;
+ sum = rx_tx_addr[1] + rx_tx_addr[2] + rx_tx_addr[3];
+ // Base row is defined by lowest 2 bits
+ const uint8_t *fh_row = freq_hopping[sum & 0x03];
+ // Higher 3 bits define increment to corresponding row
+ uint8_t increment = (sum & 0x1e) >> 2;
+ for (uint8_t i = 0; i < 16; ++i) {
+ uint8_t val = fh_row[i] + increment;
+ // Strange avoidance of channels divisible by 16
+ hopping_frequency[i] = (val & 0x0f) ? val : val - 3;
+ }
+}
+
+void add_pkt_checksum()
+{
+ uint8_t sum = 0;
+ for (uint8_t i = 0; i < 15; ++i)
+ sum += packet[i];
+ packet[15] = sum;
+}
+
+void send_packet(uint8_t bind)
+{
+ uint8_t flags2=0;
+ if (bind)
+ {
+ flags = V2X2_FLAG_BIND;
+ packet[0] = 0;
+ packet[1] = 0;
+ packet[2] = 0;
+ packet[3] = 0;
+ packet[4] = 0;
+ packet[5] = 0;
+ packet[6] = 0;
+ }
+ else
+ {
+ packet[0] = convert_channel_8b(THROTTLE);
+ packet[1] = convert_channel_s8b(RUDDER);
+ packet[2] = convert_channel_s8b(ELEVATOR);
+ packet[3] = convert_channel_s8b(AILERON);
+ // Trims, middle is 0x40
+ packet[4] = 0x40; // yaw
+ packet[5] = 0x40; // pitch
+ packet[6] = 0x40; // roll
+
+ //Flags
+ // Channel 5
+ if (Servo_data[AUX1] > PPM_SWITCH)
+ flags |= V2X2_FLAG_FLIP;
+ // Channel 6
+ if (Servo_data[AUX2] > PPM_SWITCH)
+ flags |= V2X2_FLAG_LIGHT;
+ // Channel 7
+ if (Servo_data[AUX3] > PPM_SWITCH)
+ flags |= V2X2_FLAG_CAMERA;
+ // Channel 8
+ if (Servo_data[AUX4] > PPM_SWITCH)
+ flags |= V2X2_FLAG_VIDEO;
+
+ //Flags2
+ // Channel 9
+ if (Servo_data[AUX5] > PPM_SWITCH)
+ flags2 = V2X2_FLAG_HEADLESS;
+ // Channel 10
+ if (Servo_data[AUX6] > PPM_SWITCH)
+ flags2 |= V2X2_FLAG_MAG_CAL_X;
+ // Channel 11
+ if (Servo_data[AUX7] > PPM_SWITCH)
+ flags2 |= V2X2_FLAG_MAG_CAL_Y;
+ }
+ // TX id
+ packet[7] = rx_tx_addr[1];
+ packet[8] = rx_tx_addr[2];
+ packet[9] = rx_tx_addr[3];
+ // empty
+ packet[10] = flags2;
+ packet[11] = 0x00;
+ packet[12] = 0x00;
+ packet[13] = 0x00;
+ //
+ packet[14] = flags;
+ add_pkt_checksum();
+
+ packet_sent = 0;
+ uint8_t rf_ch = hopping_frequency[hopping_frequency_no >> 1];
+ hopping_frequency_no = (hopping_frequency_no + 1) & 0x1F;
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_ch);
+ NRF24L01_FlushTx();
+ NRF24L01_WritePayload(packet, V2X2_PAYLOADSIZE);
+ ++packet_counter;
+ packet_sent = 1;
+
+ if (! hopping_frequency_no)
+ NRF24L01_SetPower();
+}
+
+uint16_t ReadV2x2()
+{
+ switch (phase) {
+ case V202_INIT2:
+ V202_init2();
+ phase = V202_BIND2;
+ return 150;
+ break;
+ case V202_INIT2_NO_BIND:
+ V202_init2();
+ phase = V202_DATA;
+ return 150;
+ break;
+ case V202_BIND2:
+ if (packet_sent && NRF24L01_packet_ack() != PKT_ACKED) {
+ return PACKET_CHKTIME;
+ }
+ send_packet(1);
+ if (--counter == 0) {
+ phase = V202_DATA;
+ BIND_DONE;
+ }
+ break;
+ case V202_DATA:
+ if (packet_sent && NRF24L01_packet_ack() != PKT_ACKED) {
+ return PACKET_CHKTIME;
+ }
+ send_packet(0);
+ break;
+ }
+ // Packet every 4ms
+ return PACKET_PERIOD;
+}
+
+uint16_t initV2x2()
+{
+ flags=0;
+ packet_counter = 0;
+ v202_init();
+ //
+ if (IS_AUTOBIND_FLAG_on)
+ {
+ counter = BIND_COUNT;
+ phase = V202_INIT2;
+ }
+ else
+ phase = V202_INIT2_NO_BIND;
+ set_tx_id();
+ return 50000;
+}
+
+#endif
diff --git a/Multiprotocol/YD717_nrf24l01.ino b/Multiprotocol/YD717_nrf24l01.ino
new file mode 100644
index 0000000..ed43851
--- /dev/null
+++ b/Multiprotocol/YD717_nrf24l01.ino
@@ -0,0 +1,256 @@
+/*
+ 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 .
+ */
+
+#if defined(YD717_NRF24L01_INO)
+
+#include "iface_nrf24l01.h"
+
+#define YD717_BIND_COUNT 60
+#define YD717_PACKET_PERIOD 8000 // Timeout for callback in uSec, 8ms=8000us for YD717
+#define YD717_INITIAL_WAIT 50000 // Initial wait before starting callbacks
+#define YD717_PACKET_CHKTIME 500 // Time to wait if packet not yet acknowledged or timed out
+
+// Stock tx fixed frequency is 0x3C. Receiver only binds on this freq.
+#define RF_CHANNEL 0x3C
+
+#define YD717_FLAG_FLIP 0x0F
+#define YD717_FLAG_LIGHT 0x80
+#define YD717_FLAG_PICTURE 0x40
+#define YD717_FLAG_VIDEO 0x20
+#define YD717_FLAG_HEADLESS 0x10
+
+#define YD717_PAYLOADSIZE 8 // receive data pipes set to this size, but unused
+//#define YD717_MAX_PACKET_SIZE 9 // YD717 packets have 8-byte payload, Syma X4 is 9
+
+enum {
+ YD717_INIT1 = 0,
+ YD717_BIND2,
+ YD717_BIND3,
+ YD717_DATA
+};
+
+void yd717_send_packet(uint8_t bind)
+{
+ uint8_t rudder_trim, elevator_trim, aileron_trim;
+ if (bind)
+ {
+ packet[0]= rx_tx_addr[0]; // send data phase address in first 4 bytes
+ packet[1]= rx_tx_addr[1];
+ packet[2]= rx_tx_addr[2];
+ packet[3]= rx_tx_addr[3];
+ packet[4] = 0x56;
+ packet[5] = 0xAA;
+ packet[6] = (sub_protocol == NIHUI) ? 0x00 : 0x32;
+ packet[7] = 0x00;
+ }
+ else
+ {
+ // Throttle
+ packet[0] = convert_channel_8b(THROTTLE);
+ // Rudder
+ if( sub_protocol==XINXUN )
+ {
+ rudder = convert_channel_8b(RUDDER);
+ rudder_trim = (0xff - rudder) >> 1;
+ }
+ else
+ {
+ rudder = 0xff - convert_channel_8b(RUDDER);
+ rudder_trim = rudder >> 1;
+ }
+ packet[1] = rudder;
+ // Elevator
+ elevator = convert_channel_8b(ELEVATOR);
+ elevator_trim = elevator >> 1;
+ packet[3] = elevator;
+ // Aileron
+ aileron = 0xff - convert_channel_8b(AILERON);
+ aileron_trim = aileron >> 1;
+ packet[4] = aileron;
+ // Trims
+ if( sub_protocol == YD717 )
+ {
+ packet[2] = elevator_trim;
+ packet[5] = aileron_trim;
+ packet[6] = rudder_trim;
+ }
+ else
+ {
+ packet[2] = rudder_trim;
+ packet[5] = elevator_trim;
+ packet[6] = aileron_trim;
+ }
+ // Flags
+ // Channel 5
+ if (Servo_data[AUX1] > PPM_SWITCH)
+ flags = YD717_FLAG_FLIP;
+ else
+ flags=0;
+ // Channel 6
+ if (Servo_data[AUX2] > PPM_SWITCH)
+ flags |= YD717_FLAG_LIGHT;
+ // Channel 7
+ if (Servo_data[AUX3] > PPM_SWITCH)
+ flags |= YD717_FLAG_PICTURE;
+ // Channel 8
+ if (Servo_data[AUX4] > PPM_SWITCH)
+ flags |= YD717_FLAG_VIDEO;
+ // Channel 9
+ if (Servo_data[AUX5] > PPM_SWITCH)
+ flags |= YD717_FLAG_HEADLESS;
+ packet[7] = flags;
+ }
+
+ // clear packet status bits and TX FIFO
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, (BV(NRF24L01_07_TX_DS) | BV(NRF24L01_07_MAX_RT)));
+ NRF24L01_FlushTx();
+
+ if( sub_protocol == YD717 )
+ NRF24L01_WritePayload(packet, 8);
+ else
+ {
+ packet[8] = packet[0]; // checksum
+ for(uint8_t i=1; i < 8; i++)
+ packet[8] += packet[i];
+ packet[8] = ~packet[8];
+ NRF24L01_WritePayload(packet, 9);
+ }
+
+ NRF24L01_SetPower(); // Set tx_power
+}
+
+void yd717_init()
+{
+ NRF24L01_Initialize();
+
+ // CRC, radio on
+ NRF24L01_SetTxRxMode(TX_EN);
+ NRF24L01_WriteReg(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_PWR_UP));
+ NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x3F); // Auto Acknoledgement on all data pipes
+ NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x3F); // Enable all data pipes
+ NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address
+ NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0x1A); // 500uS retransmit t/o, 10 tries
+ NRF24L01_WriteReg(NRF24L01_05_RF_CH, RF_CHANNEL); // Channel 3C
+ NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps
+ NRF24L01_SetPower();
+ NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit
+ NRF24L01_WriteReg(NRF24L01_0C_RX_ADDR_P2, 0xC3); // LSB byte of pipe 2 receive address
+ NRF24L01_WriteReg(NRF24L01_0D_RX_ADDR_P3, 0xC4);
+ NRF24L01_WriteReg(NRF24L01_0E_RX_ADDR_P4, 0xC5);
+ NRF24L01_WriteReg(NRF24L01_0F_RX_ADDR_P5, 0xC6);
+ NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, YD717_PAYLOADSIZE); // bytes of data payload for pipe 1
+ NRF24L01_WriteReg(NRF24L01_12_RX_PW_P1, YD717_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_13_RX_PW_P2, YD717_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_14_RX_PW_P3, YD717_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_15_RX_PW_P4, YD717_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_16_RX_PW_P5, YD717_PAYLOADSIZE);
+ NRF24L01_WriteReg(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here
+
+ NRF24L01_Activate(0x73); // Activate feature register
+ NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x3F); // Enable dynamic payload length on all pipes
+ NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x07); // Set feature bits on
+ NRF24L01_Activate(0x73);
+
+ // set tx id
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
+}
+
+void YD717_init1()
+{
+ // for bind packets set address to prearranged value known to receiver
+ uint8_t bind_rx_tx_addr[] = {0x65, 0x65, 0x65, 0x65, 0x65};
+ if( sub_protocol==SYMAX2 )
+ for(uint8_t i=0; i < 5; i++)
+ bind_rx_tx_addr[i] = 0x60;
+ else
+ if( sub_protocol==NIHUI )
+ for(uint8_t i=0; i < 5; i++)
+ bind_rx_tx_addr[i] = 0x64;
+
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, bind_rx_tx_addr, 5);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, bind_rx_tx_addr, 5);
+}
+
+void YD717_init2()
+{
+ // set rx/tx address for data phase
+ NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, rx_tx_addr, 5);
+ NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
+}
+
+uint16_t yd717_callback()
+{
+ switch (phase)
+ {
+ case YD717_INIT1:
+ yd717_send_packet(0); // receiver doesn't re-enter bind mode if connection lost...check if already bound
+ phase = YD717_BIND3;
+ break;
+ case YD717_BIND2:
+ if (counter == 0)
+ {
+ if (NRF24L01_packet_ack() == PKT_PENDING)
+ return YD717_PACKET_CHKTIME; // packet send not yet complete
+ YD717_init2(); // change to data phase rx/tx address
+ yd717_send_packet(0);
+ phase = YD717_BIND3;
+ }
+ else
+ {
+ if (NRF24L01_packet_ack() == PKT_PENDING)
+ return YD717_PACKET_CHKTIME; // packet send not yet complete;
+ yd717_send_packet(1);
+ counter--;
+ }
+ break;
+ case YD717_BIND3:
+ switch (NRF24L01_packet_ack())
+ {
+ case PKT_PENDING:
+ return YD717_PACKET_CHKTIME; // packet send not yet complete
+ case PKT_ACKED:
+ phase = YD717_DATA;
+ BIND_DONE; // bind complete
+ break;
+ case PKT_TIMEOUT:
+ YD717_init1(); // change to bind rx/tx address
+ counter = YD717_BIND_COUNT;
+ phase = YD717_BIND2;
+ yd717_send_packet(1);
+ }
+ break;
+ case YD717_DATA:
+ if (NRF24L01_packet_ack() == PKT_PENDING)
+ return YD717_PACKET_CHKTIME; // packet send not yet complete
+ yd717_send_packet(0);
+ break;
+ }
+ return YD717_PACKET_PERIOD; // Packet every 8ms
+}
+
+uint16_t initYD717()
+{
+ rx_tx_addr[4] = 0xC1; // always uses first data port
+ flags = 0;
+ yd717_init();
+ phase = YD717_INIT1;
+ BIND_IN_PROGRESS; // autobind protocol
+
+ // Call callback in 50ms
+ return YD717_INITIAL_WAIT;
+}
+
+#endif
diff --git a/Multiprotocol/iface_a7105.h b/Multiprotocol/iface_a7105.h
new file mode 100644
index 0000000..f8792d9
--- /dev/null
+++ b/Multiprotocol/iface_a7105.h
@@ -0,0 +1,95 @@
+/*
+ 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 .
+ */
+
+#ifndef _IFACE_A7105_H_
+#define _IFACE_A7105_H_
+
+enum A7105_State {
+ A7105_SLEEP = 0x80,
+ A7105_IDLE = 0x90,
+ A7105_STANDBY = 0xA0,
+ A7105_PLL = 0xB0,
+ A7105_RX = 0xC0,
+ A7105_TX = 0xD0,
+ A7105_RST_WRPTR = 0xE0,
+ A7105_RST_RDPTR = 0xF0,
+};
+
+enum {
+ A7105_00_MODE = 0x00,
+ A7105_01_MODE_CONTROL = 0x01,
+ A7105_02_CALC = 0x02,
+ A7105_03_FIFOI = 0x03,
+ A7105_04_FIFOII = 0x04,
+ A7105_05_FIFO_DATA = 0x05,
+ A7105_06_ID_DATA = 0x06,
+ A7105_07_RC_OSC_I = 0x07,
+ A7105_08_RC_OSC_II = 0x08,
+ A7105_09_RC_OSC_III = 0x09,
+ A7105_0A_CK0_PIN = 0x0A,
+ A7105_0B_GPIO1_PIN1 = 0x0B,
+ A7105_0C_GPIO2_PIN_II = 0x0C,
+ A7105_0D_CLOCK = 0x0D,
+ A7105_0E_DATA_RATE = 0x0E,
+ A7105_0F_PLL_I = 0x0F,
+ A7105_10_PLL_II = 0x10,
+ A7105_11_PLL_III = 0x11,
+ A7105_12_PLL_IV = 0x12,
+ A7105_13_PLL_V = 0x13,
+ A7105_14_TX_I = 0x14,
+ A7105_15_TX_II = 0x15,
+ A7105_16_DELAY_I = 0x16,
+ A7105_17_DELAY_II = 0x17,
+ A7105_18_RX = 0x18,
+ A7105_19_RX_GAIN_I = 0x19,
+ A7105_1A_RX_GAIN_II = 0x1A,
+ A7105_1B_RX_GAIN_III = 0x1B,
+ A7105_1C_RX_GAIN_IV = 0x1C,
+ A7105_1D_RSSI_THOLD = 0x1D,
+ A7105_1E_ADC = 0x1E,
+ A7105_1F_CODE_I = 0x1F,
+ A7105_20_CODE_II = 0x20,
+ A7105_21_CODE_III = 0x21,
+ A7105_22_IF_CALIB_I = 0x22,
+ A7105_23_IF_CALIB_II = 0x23,
+ A7105_24_VCO_CURCAL = 0x24,
+ A7105_25_VCO_SBCAL_I = 0x25,
+ A7105_26_VCO_SBCAL_II = 0x26,
+ A7105_27_BATTERY_DET = 0x27,
+ A7105_28_TX_TEST = 0x28,
+ A7105_29_RX_DEM_TEST_I = 0x29,
+ A7105_2A_RX_DEM_TEST_II = 0x2A,
+ A7105_2B_CPC = 0x2B,
+ A7105_2C_XTAL_TEST = 0x2C,
+ A7105_2D_PLL_TEST = 0x2D,
+ A7105_2E_VCO_TEST_I = 0x2E,
+ A7105_2F_VCO_TEST_II = 0x2F,
+ A7105_30_IFAT = 0x30,
+ A7105_31_RSCALE = 0x31,
+ A7105_32_FILTER_TEST = 0x32,
+};
+#define A7105_0F_CHANNEL A7105_0F_PLL_I
+
+enum A7105_MASK {
+ A7105_MASK_FBCF = 1 << 4,
+ A7105_MASK_VBCF = 1 << 3,
+};
+
+enum {
+ INIT_FLYSKY,
+ INIT_HUBSAN
+};
+
+#endif
\ No newline at end of file
diff --git a/Multiprotocol/iface_cc2500.h b/Multiprotocol/iface_cc2500.h
new file mode 100644
index 0000000..8bcd506
--- /dev/null
+++ b/Multiprotocol/iface_cc2500.h
@@ -0,0 +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.
+
+ 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 .
+ */
+
+#ifndef _IFACE_CC2500_H_
+#define _IFACE_CC2500_H_
+
+enum {
+ CC2500_00_IOCFG2 = 0x00, // GDO2 output pin configuration
+ CC2500_01_IOCFG1 = 0x01, // GDO1 output pin configuration
+ CC2500_02_IOCFG0 = 0x02, // GDO0 output pin configuration
+ CC2500_03_FIFOTHR = 0x03, // RX FIFO and TX FIFO thresholds
+ CC2500_04_SYNC1 = 0x04, // Sync word, high byte
+ CC2500_05_SYNC0 = 0x05, // Sync word, low byte
+ CC2500_06_PKTLEN = 0x06, // Packet length
+ CC2500_07_PKTCTRL1 = 0x07, // Packet automation control
+ CC2500_08_PKTCTRL0 = 0x08, // Packet automation control
+ CC2500_09_ADDR = 0x09, // Device address
+ CC2500_0A_CHANNR = 0x0A, // Channel number
+ CC2500_0B_FSCTRL1 = 0x0B, // Frequency synthesizer control
+ CC2500_0C_FSCTRL0 = 0x0C, // Frequency synthesizer control
+ CC2500_0D_FREQ2 = 0x0D, // Frequency control word, high byte
+ CC2500_0E_FREQ1 = 0x0E, // Frequency control word, middle byte
+ CC2500_0F_FREQ0 = 0x0F, // Frequency control word, low byte
+ CC2500_10_MDMCFG4 = 0x10, // Modem configuration
+ CC2500_11_MDMCFG3 = 0x11, // Modem configuration
+ CC2500_12_MDMCFG2 = 0x12, // Modem configuration
+ CC2500_13_MDMCFG1 = 0x13, // Modem configuration
+ CC2500_14_MDMCFG0 = 0x14, // Modem configuration
+ CC2500_15_DEVIATN = 0x15, // Modem deviation setting
+ CC2500_16_MCSM2 = 0x16, // Main Radio Cntrl State Machine config
+ CC2500_17_MCSM1 = 0x17, // Main Radio Cntrl State Machine config
+ CC2500_18_MCSM0 = 0x18, // Main Radio Cntrl State Machine config
+ CC2500_19_FOCCFG = 0x19, // Frequency Offset Compensation config
+ CC2500_1A_BSCFG = 0x1A, // Bit Synchronization configuration
+ CC2500_1B_AGCCTRL2 = 0x1B, // AGC control
+ CC2500_1C_AGCCTRL1 = 0x1C, // AGC control
+ CC2500_1D_AGCCTRL0 = 0x1D, // AGC control
+ CC2500_1E_WOREVT1 = 0x1E, // High byte Event 0 timeout
+ CC2500_1F_WOREVT0 = 0x1F, // Low byte Event 0 timeout
+ CC2500_20_WORCTRL = 0x20, // Wake On Radio control
+ CC2500_21_FREND1 = 0x21, // Front end RX configuration
+ CC2500_22_FREND0 = 0x22, // Front end TX configuration
+ CC2500_23_FSCAL3 = 0x23, // Frequency synthesizer calibration
+ CC2500_24_FSCAL2 = 0x24, // Frequency synthesizer calibration
+ CC2500_25_FSCAL1 = 0x25, // Frequency synthesizer calibration
+ CC2500_26_FSCAL0 = 0x26, // Frequency synthesizer calibration
+ CC2500_27_RCCTRL1 = 0x27, // RC oscillator configuration
+ CC2500_28_RCCTRL0 = 0x28, // RC oscillator configuration
+ CC2500_29_FSTEST = 0x29, // Frequency synthesizer cal control
+ CC2500_2A_PTEST = 0x2A, // Production test
+ CC2500_2B_AGCTEST = 0x2B, // AGC test
+ CC2500_2C_TEST2 = 0x2C, // Various test settings
+ CC2500_2D_TEST1 = 0x2D, // Various test settings
+ CC2500_2E_TEST0 = 0x2E, // Various test settings
+
+// Status registers
+ CC2500_30_PARTNUM = 0x30, // Part number
+ CC2500_31_VERSION = 0x31, // Current version number
+ CC2500_32_FREQEST = 0x32, // Frequency offset estimate
+ CC2500_33_LQI = 0x33, // Demodulator estimate for link quality
+ CC2500_34_RSSI = 0x34, // Received signal strength indication
+ CC2500_35_MARCSTATE = 0x35, // Control state machine state
+ CC2500_36_WORTIME1 = 0x36, // High byte of WOR timer
+ CC2500_37_WORTIME0 = 0x37, // Low byte of WOR timer
+ CC2500_38_PKTSTATUS = 0x38, // Current GDOx status and packet status
+ CC2500_39_VCO_VC_DAC = 0x39, // Current setting from PLL cal module
+ CC2500_3A_TXBYTES = 0x3A, // Underflow and # of bytes in TXFIFO
+ CC2500_3B_RXBYTES = 0x3B, // Overflow and # of bytes in RXFIFO
+
+// Multi byte memory locations
+ CC2500_3E_PATABLE = 0x3E,
+ CC2500_3F_TXFIFO = 0x3F,
+ CC2500_3F_RXFIFO = 0x3F,
+};
+
+// Definitions for burst/single access to registers
+#define CC2500_WRITE_SINGLE 0x00
+#define CC2500_WRITE_BURST 0x40
+#define CC2500_READ_SINGLE 0x80
+#define CC2500_READ_BURST 0xC0
+
+// Strobe commands
+#define CC2500_SRES 0x30 // Reset chip.
+#define CC2500_SFSTXON 0x31 // Enable and calibrate frequency synthesizer (if MCSM0.FS_AUTOCAL=1).
+ // If in RX/TX: Go to a wait state where only the synthesizer is
+ // running (for quick RX / TX turnaround).
+#define CC2500_SXOFF 0x32 // Turn off crystal oscillator.
+#define CC2500_SCAL 0x33 // Calibrate frequency synthesizer and turn it off
+ // (enables quick start).
+#define CC2500_SRX 0x34 // Enable RX. Perform calibration first if coming from IDLE and
+ // MCSM0.FS_AUTOCAL=1.
+#define CC2500_STX 0x35 // In IDLE state: Enable TX. Perform calibration first if
+ // MCSM0.FS_AUTOCAL=1. If in RX state and CCA is enabled:
+ // Only go to TX if channel is clear.
+#define CC2500_SIDLE 0x36 // Exit RX / TX, turn off frequency synthesizer and exit
+ // Wake-On-Radio mode if applicable.
+#define CC2500_SAFC 0x37 // Perform AFC adjustment of the frequency synthesizer
+#define CC2500_SWOR 0x38 // Start automatic RX polling sequence (Wake-on-Radio)
+#define CC2500_SPWD 0x39 // Enter power down mode when CSn goes high.
+#define CC2500_SFRX 0x3A // Flush the RX FIFO buffer.
+#define CC2500_SFTX 0x3B // Flush the TX FIFO buffer.
+#define CC2500_SWORRST 0x3C // Reset real time clock.
+#define CC2500_SNOP 0x3D // No operation. May be used to pad strobe commands to two
+ // bytes for simpler software.
+//----------------------------------------------------------------------------------
+// Chip Status Byte
+//----------------------------------------------------------------------------------
+
+// Bit fields in the chip status byte
+#define CC2500_STATUS_CHIP_RDYn_BM 0x80
+#define CC2500_STATUS_STATE_BM 0x70
+#define CC2500_STATUS_FIFO_BYTES_AVAILABLE_BM 0x0F
+
+// Chip states
+#define CC2500_STATE_IDLE 0x00
+#define CC2500_STATE_RX 0x10
+#define CC2500_STATE_TX 0x20
+#define CC2500_STATE_FSTXON 0x30
+#define CC2500_STATE_CALIBRATE 0x40
+#define CC2500_STATE_SETTLING 0x50
+#define CC2500_STATE_RX_OVERFLOW 0x60
+#define CC2500_STATE_TX_UNDERFLOW 0x70
+
+//----------------------------------------------------------------------------------
+// Other register bit fields
+//----------------------------------------------------------------------------------
+#define CC2500_LQI_CRC_OK_BM 0x80
+#define CC2500_LQI_EST_BM 0x7F
+
+//void CC2500_WriteReg(u8 addr, u8 data);
+//u8 CC2500_ReadReg(u8 addr);
+//void CC2500_Reset();
+//void CC2500_Strobe(u8 cmd);
+//void CC2500_WriteData(u8 *packet, u8 length);
+//void CC2500_ReadData(u8 *dpbuffer, int len);
+//void CC2500_SetTxRxMode(enum TXRX_State);
+#endif
diff --git a/Multiprotocol/iface_cyrf6936.h b/Multiprotocol/iface_cyrf6936.h
new file mode 100644
index 0000000..567014b
--- /dev/null
+++ b/Multiprotocol/iface_cyrf6936.h
@@ -0,0 +1,99 @@
+/*
+ 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 .
+ */
+
+#ifndef _IFACE_CYRF6936_H_
+#define _IFACE_CYRF6936_H_
+enum {
+ CYRF_00_CHANNEL = 0x00,
+ CYRF_01_TX_LENGTH = 0x01,
+ CYRF_02_TX_CTRL = 0x02,
+ CYRF_03_TX_CFG = 0x03,
+ CYRF_04_TX_IRQ_STATUS = 0x04,
+ CYRF_05_RX_CTRL = 0x05,
+ CYRF_06_RX_CFG = 0x06,
+ CYRF_07_RX_IRQ_STATUS = 0x07,
+ CYRF_08_RX_STATUS = 0x08,
+ CYRF_09_RX_COUNT = 0x09,
+ CYRF_0A_RX_LENGTH = 0x0A,
+ CYRF_0B_PWR_CTRL = 0x0B,
+ CYRF_0C_XTAL_CTRL = 0x0C,
+ CYRF_0D_IO_CFG = 0x0D,
+ CYRF_0E_GPIO_CTRL = 0x0E,
+ CYRF_0F_XACT_CFG = 0x0F,
+ CYRF_10_FRAMING_CFG = 0x10,
+ CYRF_11_DATA32_THOLD = 0x11,
+ CYRF_12_DATA64_THOLD = 0x12,
+ CYRF_13_RSSI = 0x13,
+ CYRF_14_EOP_CTRL = 0x14,
+ CYRF_15_CRC_SEED_LSB = 0x15,
+ CYRF_16_CRC_SEED_MSB = 0x16,
+ CYRF_17_TX_CRC_LSB = 0x17,
+ CYRF_18_TX_CRC_MSB = 0x18,
+ CYRF_19_RX_CRC_LSB = 0x19,
+ CYRF_1A_RX_CRC_MSB = 0x1A,
+ CYRF_1B_TX_OFFSET_LSB = 0x1B,
+ CYRF_1C_TX_OFFSET_MSB = 0x1C,
+ CYRF_1D_MODE_OVERRIDE = 0x1D,
+ CYRF_1E_RX_OVERRIDE = 0x1E,
+ CYRF_1F_TX_OVERRIDE = 0x1F,
+ /*Register Files */
+ CYRF_20_TX_BUFFER = 0x20,
+ CYRF_21_RX_BUFFER = 0x21,
+ CYRF_22_SOP_CODE = 0x22,
+ CYRF_23_DATA_CODE = 0x23,
+ CYRF_24_PREAMBLE = 0x24,
+ CYRF_25_MFG_ID = 0x25,
+ /*****************/
+ CYRF_26_XTAL_CFG = 0x26,
+ CYRF_27_CLK_OVERRIDE = 0x27,
+ CYRF_28_CLK_EN = 0x28,
+ CYRF_29_RX_ABORT = 0x29,
+ CYRF_32_AUTO_CAL_TIME = 0x32,
+ CYRF_35_AUTOCAL_OFFSET = 0x35,
+ CYRF_39_ANALOG_CTRL = 0x39,
+};
+
+enum CYRF_PWR {
+ CYRF_PWR_100MW,
+ CYRF_PWR_10MW,
+ CYRF_PWR_DEFAULT,
+};
+
+
+
+/* SPI CYRF6936 */
+/*
+void CYRF_Initialize();
+int CYRF_Reset();
+void CYRF_GetMfgData(u8 data[]);
+
+void CYRF_SetTxRxMode(enum TXRX_State);
+void CYRF_ConfigRFChannel(u8 ch);
+void CYRF_SetPower(u8 power);
+void CYRF_ConfigCRCSeed(u16 crc);
+void CYRF_StartReceive();
+void CYRF_ConfigSOPCode(const u8 *sopcodes);
+void CYRF_ConfigDataCode(const u8 *datacodes, u8 len);
+u8 CYRF_ReadRSSI(u32 dodummyread);
+void CYRF_ReadDataPacket(u8 dpbuffer[]);
+void CYRF_WriteDataPacket(const u8 dpbuffer[]);
+void CYRF_WriteDataPacketLen(const u8 dpbuffer[], u8 len);
+void CYRF_WriteRegister(u8 address, u8 data);
+u8 CYRF_ReadRegister(u8 address);
+void CYRF_WritePreamble(u32 preamble);
+u8 CYRF_MaxPower();
+void CYRF_FindBestChannels(u8 *channels, u8 len, u8 minspace, u8 minchan, u8 maxchan);
+*/
+#endif
diff --git a/Multiprotocol/iface_nrf24l01.h b/Multiprotocol/iface_nrf24l01.h
new file mode 100644
index 0000000..1acf438
--- /dev/null
+++ b/Multiprotocol/iface_nrf24l01.h
@@ -0,0 +1,119 @@
+/*
+ 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 .
+ */
+
+#ifndef _IFACE_NRF24L01_H_
+#define _IFACE_NRF24L01_H_
+// Register map
+enum {
+ NRF24L01_00_CONFIG = 0x00,
+ NRF24L01_01_EN_AA = 0x01,
+ NRF24L01_02_EN_RXADDR = 0x02,
+ NRF24L01_03_SETUP_AW = 0x03,
+ NRF24L01_04_SETUP_RETR = 0x04,
+ NRF24L01_05_RF_CH = 0x05,
+ NRF24L01_06_RF_SETUP = 0x06,
+ NRF24L01_07_STATUS = 0x07,
+ NRF24L01_08_OBSERVE_TX = 0x08,
+ NRF24L01_09_CD = 0x09,
+ NRF24L01_0A_RX_ADDR_P0 = 0x0A,
+ NRF24L01_0B_RX_ADDR_P1 = 0x0B,
+ NRF24L01_0C_RX_ADDR_P2 = 0x0C,
+ NRF24L01_0D_RX_ADDR_P3 = 0x0D,
+ NRF24L01_0E_RX_ADDR_P4 = 0x0E,
+ NRF24L01_0F_RX_ADDR_P5 = 0x0F,
+ NRF24L01_10_TX_ADDR = 0x10,
+ NRF24L01_11_RX_PW_P0 = 0x11,
+ NRF24L01_12_RX_PW_P1 = 0x12,
+ NRF24L01_13_RX_PW_P2 = 0x13,
+ NRF24L01_14_RX_PW_P3 = 0x14,
+ NRF24L01_15_RX_PW_P4 = 0x15,
+ NRF24L01_16_RX_PW_P5 = 0x16,
+ NRF24L01_17_FIFO_STATUS = 0x17,
+ NRF24L01_1C_DYNPD = 0x1C,
+ NRF24L01_1D_FEATURE = 0x1D,
+ //Instructions
+ NRF24L01_61_RX_PAYLOAD = 0x61,
+ NRF24L01_A0_TX_PAYLOAD = 0xA0,
+ NRF24L01_E1_FLUSH_TX = 0xE1,
+ NRF24L01_E2_FLUSH_RX = 0xE2,
+ NRF24L01_E3_REUSE_TX_PL = 0xE3,
+ NRF24L01_50_ACTIVATE = 0x50,
+ NRF24L01_60_R_RX_PL_WID = 0x60,
+ NRF24L01_B0_TX_PYLD_NOACK = 0xB0,
+ NRF24L01_FF_NOP = 0xFF,
+ NRF24L01_A8_W_ACK_PAYLOAD0 = 0xA8,
+ NRF24L01_A8_W_ACK_PAYLOAD1 = 0xA9,
+ NRF24L01_A8_W_ACK_PAYLOAD2 = 0xAA,
+ NRF24L01_A8_W_ACK_PAYLOAD3 = 0xAB,
+ NRF24L01_A8_W_ACK_PAYLOAD4 = 0xAC,
+ NRF24L01_A8_W_ACK_PAYLOAD5 = 0xAD,
+};
+
+// Bit mnemonics
+enum {
+ NRF24L01_00_MASK_RX_DR = 6,
+ NRF24L01_00_MASK_TX_DS = 5,
+ NRF24L01_00_MASK_MAX_RT = 4,
+ NRF24L01_00_EN_CRC = 3,
+ NRF24L01_00_CRCO = 2,
+ NRF24L01_00_PWR_UP = 1,
+ NRF24L01_00_PRIM_RX = 0,
+
+ NRF24L01_07_RX_DR = 6,
+ NRF24L01_07_TX_DS = 5,
+ NRF24L01_07_MAX_RT = 4,
+
+ NRF2401_1D_EN_DYN_ACK = 0,
+ NRF2401_1D_EN_ACK_PAY = 1,
+ NRF2401_1D_EN_DPL = 2,
+};
+
+// Bitrates
+enum {
+ NRF24L01_BR_1M = 0,
+ NRF24L01_BR_2M,
+ NRF24L01_BR_250K,
+ NRF24L01_BR_RSVD
+};
+
+/* Instruction Mnemonics */
+#define R_REGISTER 0x00
+#define W_REGISTER 0x20
+#define REGISTER_MASK 0x1F
+#define ACTIVATE 0x50
+#define R_RX_PL_WID 0x60
+#define R_RX_PAYLOAD 0x61
+#define W_TX_PAYLOAD 0xA0
+#define W_ACK_PAYLOAD 0xA8
+#define FLUSH_TX 0xE1
+#define FLUSH_RX 0xE2
+#define REUSE_TX_PL 0xE3
+//#define NOP 0xFF
+
+/*
+void NRF24L01_Initialize();
+byte NRF24L01_WriteReg(byte reg, byte data);
+byte NRF24L01_WriteRegisterMulti(byte reg, byte data[], byte length);
+byte NRF24L01_WritePayload(byte *data, byte len);
+byte NRF24L01_ReadReg(byte reg);
+byte NRF24L01_ReadRegisterMulti(byte reg, byte data[], byte length);
+byte NRF24L01_ReadPayload(byte *data, byte len);
+
+byte NRF24L01_FlushTx();
+byte NRF24L01_FlushRx();
+byte NRF24L01_Activate(byte code);
+
+*/
+#endif
\ No newline at end of file
diff --git a/Multiprotocol/multiprotocol.h b/Multiprotocol/multiprotocol.h
new file mode 100644
index 0000000..70a252e
--- /dev/null
+++ b/Multiprotocol/multiprotocol.h
@@ -0,0 +1,462 @@
+/*
+ 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 .
+ */
+
+//******************
+enum PROTOCOLS
+{
+ MODE_SERIAL = 0, // Serial commands
+ MODE_FLYSKY = 1, // =>A7105 / FLYSKY protocol
+ MODE_HUBSAN = 2, // =>A7105 / HUBSAN protocol
+ MODE_FRSKY = 3, // =>CC2500 / FRSKY protocol
+ MODE_HISKY = 4, // =>NRF24L01 / HISKY protocol
+ MODE_V2X2 = 5, // =>NRF24L01 / V2x2 protocol
+ MODE_DSM2 = 6, // =>CYRF6936 / DSM2 protocol
+ MODE_DEVO =7, // =>CYRF6936 / DEVO protocol
+ MODE_YD717 = 8, // =>NRF24L01 / YD717 protocol (CX10 red pcb)
+ MODE_KN = 9, // =>NRF24L01 / KN protocol
+ MODE_SYMAX = 10, // =>NRF24L01 / SYMAX protocol (SYMAX4 working)
+ MODE_SLT = 11, // =>NRF24L01 / SLT protocol
+ MODE_CX10 = 12, // =>NRF24L01 / CX-10 protocol
+ MODE_CG023 = 13, // =>NRF24L01 / CG023 protocol
+ MODE_BAYANG = 14, // =>NRF24L01 / BAYANG protocol
+ MODE_FRSKYX = 15, // =>CC2500 / FRSKYX protocol
+};
+enum Flysky
+{
+ Flysky=0,
+ V9X9=1,
+ V6X6=2,
+ V912=3
+};
+enum Hisky
+{
+ Hisky=0,
+ HK310=1
+};
+enum DSM2{
+ DSM2=0,
+ DSMX=1
+};
+enum YD717
+{
+ YD717=0,
+ SKYWLKR=1,
+ SYMAX2=2,
+ XINXUN=3,
+ NIHUI=4
+};
+enum SYMAX
+{
+ SYMAX=0,
+ SYMAX5C=1,
+};
+
+enum CX10 {
+ CX10_GREEN = 0,
+ CX10_BLUE, // also compatible with CX10-A, CX12
+ DM007
+};
+
+enum CG023 {
+ CG023 = 0,
+ YD829 = 1
+};
+
+#define PPM_MIN_COMMAND 1250
+#define PPM_SWITCH 1550
+#define PPM_MAX_COMMAND 1750
+
+enum TXRX_State {
+ TXRX_OFF,
+ TX_EN,
+ RX_EN
+};
+
+// Packet ack status values
+enum {
+ PKT_PENDING = 0,
+ PKT_ACKED,
+ PKT_TIMEOUT
+};
+
+//*******************
+//*** Pinouts ***
+//*******************
+//#define BIND_pin 13
+#define LED_pin 13 //Promini original led on B5
+//
+#define PPM_pin 3 //PPM -D3
+#define SDI_pin 5 //SDIO-D5
+#define SCLK_pin 4 //SCK-D4
+#define CS_pin 2 //CS-D2
+#define SDO_pin 6 //D6
+//
+#define CTRL1 1 //C1 (A1)
+#define CTRL2 2 //C2 (A2)
+//
+#define CTRL1_on PORTC |= _BV(1)
+#define CTRL1_off PORTC &= ~_BV(1)
+//
+#define CTRL2_on PORTC |= _BV(2)
+#define CTRL2_off PORTC &= ~_BV(2)
+//
+#define CS_on PORTD |= _BV(2) //D2
+#define CS_off PORTD &= ~_BV(2) //D2
+//
+#define SCK_on PORTD |= _BV(4) //D4
+#define SCK_off PORTD &= ~_BV(4) //D4
+//
+#define SDI_on PORTD |= _BV(5) //D5
+#define SDI_off PORTD &= ~_BV(5) //D5
+
+#define SDI_1 (PIND & (1< Reserved 0
+ Flysky 1
+ Hubsan 2
+ Frsky 3
+ Hisky 4
+ V2x2 5
+ DSM2 6
+ Devo 7
+ YD717 8
+ KN 9
+ SymaX 10
+ SLT 11
+ CX10 12
+ CG023 13
+ Bayang 14
+ BindBit=> 0x80 1=Bind/0=No
+ AutoBindBit=> 0x40 1=Yes /0=No
+ RangeCheck=> 0x20 1=Yes /0=No
+ Stream[2] = RxNum | Power | Type;
+ RxNum value is 0..15 (bits 0..3)
+ Type is 0..7 <<4 (bit 4..6)
+ sub_protocol==Flysky
+ Flysky 0
+ V9x9 1
+ V6x6 2
+ V912 3
+ sub_protocol==Hisky
+ Hisky 0
+ HK310 1
+ sub_protocol==DSM2
+ DSM2 0
+ DSMX 1
+ sub_protocol==YD717
+ YD717 0
+ SKYWLKR 1
+ SYMAX2 2
+ XINXUN 3
+ NIHUI 4
+ sub_protocol==SYMAX
+ SYMAX 0
+ SYMAX5C 1
+ sub_protocol==CX10
+ CX10_GREEN 0
+ CX10_BLUE 1 // also compatible with CX10-A, CX12
+ DM007 2
+ sub_protocol==CG023
+ CG023 0
+ YD829 1
+ Power value => 0x80 0=High/1=Low
+ Stream[3] = option_protocol;
+ option_protocol value is -127..127
+ Stream[4] to [25] = Channels
+ 16 Channels on 11 bits (0..2047)
+ 0 -125%
+ 204 -100%
+ 1024 0%
+ 1843 +100%
+ 2047 +125%
+ Channels bits are concatenated to fit in 22 bytes like in SBUS protocol
+
+
+**************************
+8 channels serial protocol
+**************************
+Serial: 125000 Baud 8n1 _ xxxx xxxx - ---
+ Channels:
+ Nbr=8
+ 10bits=0..1023
+ 0 -125%
+ 96 -100%
+ 512 0%
+ 928 +100%
+ 1023 +125%
+ Stream[0] = sub_protocol|BindBit|RangeCheckBit|AutoBindBit;
+ sub_protocol is 0..31 (bits 0..4)
+ => Reserved 0
+ Flysky 1
+ Hubsan 2
+ Frsky 3
+ Hisky 4
+ V2x2 5
+ DSM2 6
+ Devo 7
+ YD717 8
+ KN 9
+ SymaX 10
+ SLT 11
+ CX10 12
+ CG023 13
+ Bayang 14
+ BindBit=> 0x80 1=Bind/0=No
+ AutoBindBit=> 0x40 1=Yes /0=No
+ RangeCheck=> 0x20 1=Yes /0=No
+ Stream[1] = RxNum | Power | Type;
+ RxNum value is 0..15 (bits 0..3)
+ Type is 0..7 <<4 (bit 4..6)
+ sub_protocol==Flysky
+ Flysky 0
+ V9x9 1
+ V6x6 2
+ V912 3
+ sub_protocol==Hisky
+ Hisky 0
+ HK310 1
+ sub_protocol==DSM2
+ DSM2 0
+ DSMX 1
+ sub_protocol==YD717
+ YD717 0
+ SKYWLKR 1
+ SYMAX2 2
+ XINXUN 3
+ NIHUI 4
+ sub_protocol==SYMAX
+ SYMAX 0
+ SYMAX5C 1
+ sub_protocol==CX10
+ CX10_GREEN 0
+ CX10_BLUE 1 // also compatible with CX10-A, CX12
+ DM007 2
+ sub_protocol==CG023
+ CG023 0
+ YD829 1
+ Power value => 0x80 0=High/1=Low
+ Stream[2] = option_protocol;
+ option_protocol value is -127..127
+ Stream[i+3] = lowByte(channel[i]) // with i[0..7]
+ Stream[11] = highByte(channel[0])<<6 | highByte(channel[1])<<4 | highByte(channel[2])<<2 | highByte(channel[3])
+ Stream[12] = highByte(channel[4])<<6 | highByte(channel[5])<<4 | highByte(channel[6])<<2 | highByte(channel[7])
+ Stream[13] = lowByte(CRC16(Stream[0..12])
+*/
+
diff --git a/Multiprotocol/telemetry.h b/Multiprotocol/telemetry.h
new file mode 100644
index 0000000..adcdc12
--- /dev/null
+++ b/Multiprotocol/telemetry.h
@@ -0,0 +1,69 @@
+/*
+ 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 .
+ */
+
+
+void frskySendStuffed(uint8_t frame[])
+{
+
+ Serial_write(0x7E);
+ for (uint8_t i = 0; i < 9; i++) {
+ if ((frame[i] == 0x7e) || (frame[i] == 0x7d)) {
+ Serial_write(0x7D);
+ frame[i] ^= 0x20;
+ }
+ Serial_write(frame[i]);
+ }
+ Serial_write(0x7E);
+}
+
+
+
+void frskySendFrame()
+{
+ uint8_t frame[9];
+
+ 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
+ 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);
+
+}
+
+
+
+void frskyUpdate()
+{
+if(telemetry_link){
+ frskySendFrame();
+ telemetry_link=0;
+
+ }
+}