#include #include "nRF24L01.h" #include "RF24.h" #include "printf.h" #include "TimerOne.h" #define BIND_COUNT 345 // 1.5 seconds #define FIRST_PACKET_DELAY 12000 //printf inside an interrupt handler is really dangerous //this shouldn't be enabled even in debug builds without explicitly //turning it on #define dbgprintf if(0) printf #define PACKET_PERIOD 4000 // Timeout for callback in uSec #define INITIAL_WAIT 500 #define FLAG_FLIP 0x01 #define FLAG_VIDEO 0x02 #define FLAG_PICTURE 0x04 #define NUM_ACTIVE_CHANNELS 4 #define CHANNEL_DSC 255 // For code readability enum { CHANNEL1 = 0, CHANNEL2, CHANNEL3, CHANNEL4, CHANNEL5, CHANNEL6, CHANNEL7, CHANNEL8, CHANNEL9, CHANNEL10 }; #define PAYLOADSIZE 10 // receive data pipes set to this size, but unused #define MAX_PACKET_SIZE 16 // X11,X12,X5C-1 10-byte, X5C 16-byte unsigned char packet[MAX_PACKET_SIZE]; unsigned char packet_size; unsigned int counter; unsigned long packet_counter; rf24_pa_dbm_e tx_power; unsigned char throttle, rudder, elevator, aileron, flags; unsigned char rx_tx_addr[5]; // frequency channel management #define MAX_RF_CHANNELS 17 unsigned char current_chan; unsigned char chans[MAX_RF_CHANNELS]; unsigned char num_rf_channels; unsigned char phase; enum { SYMAX_INIT1 = 0, SYMAX_BIND2, SYMAX_BIND3, SYMAX_DATA }; //--------------------------------ariel----------- unsigned long btime; long period; long new_period; static unsigned long rand_seed = 0xb2c54a2ful; // Linear feedback shift register with 32-bit Xilinx polinomial x^32 + x^22 + x^2 + x + 1 static const uint32_t LFSR_FEEDBACK = 0x80200003ul; static const uint32_t LFSR_INTAP = 32-1; static void update_lfsr(uint32_t *lfsr, uint8_t b) { for (int i = 0; i < 8; ++i) { *lfsr = (*lfsr >> 1) ^ ((-(*lfsr & 1u) & LFSR_FEEDBACK) ^ ~((uint32_t)(b & 1) << LFSR_INTAP)); b >>= 1; } } unsigned long rand32_r(unsigned long *seed, unsigned char update) { if(! seed) seed = &rand_seed; update_lfsr(seed, update); return *seed; } // Set up nRF24L01 radio on SPI bus plus pins 9 & 10 RF24 radio(9,10); //for arduino uno //replace //radio.write_register --> radio.write_register //----------------------------------- // NRF24L01_SetPower(Model.tx_power);//setPALevel differents units need some adapter or change /* 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; }; */ rf24_datarate_e bit_rate; // 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 }; byte incomingByte ; boolean isX5 = true; #define NUM_OUT_CHANNELS 12 //#define NUM_VIRT_CHANNELS 10 typedef enum { PROTOCMD_INIT, PROTOCMD_DEINIT, PROTOCMD_BIND, PROTOCMD_CHECK_AUTOBIND, PROTOCMD_NUMCHAN, PROTOCMD_DEFAULT_NUMCHAN, PROTOCMD_CURRENT_ID, PROTOCMD_GETOPTIONS, PROTOCMD_SETOPTIONS, PROTOCMD_TELEMETRYSTATE, PROTOCMD_RESET }ProtoCmds; enum TXRX_State { TXRX_OFF, TX_EN, RX_EN, }; //MAX = 10000 //MIN = -10000 #define CHAN_MULTIPLIER 100 #define PCT_TO_RANGE(x) ((x) * CHAN_MULTIPLIER) #define RANGE_TO_PCT(x) ((x) / CHAN_MULTIPLIER) #define CHAN_MAX_VALUE (255)//(100 * CHAN_MULTIPLIER) #define CHAN_MIN_VALUE (0)//(-100 * CHAN_MULTIPLIER) #define NUM_CHANNELS (NUM_OUT_CHANNELS + NUM_VIRT_CHANNELS) long Channels[NUM_OUT_CHANNELS]; //------------–----------------------------------- // Bit vector from bit position #define BV(bit) (1 << bit) //static const char * const symax_opts[] = { // _tr_noop("SymaX5C"), _tr_noop("Off"), _tr_noop("On"), NULL, // NULL //}; //enum { // PROTOOPTS_X5C = 0, // LAST_PROTO_OPT, //}; //ctassert(LAST_PROTO_OPT <= NUM_PROTO_OPTS, too_many_protocol_opts); unsigned char checksum(unsigned char *data) { unsigned char sum = data[0]; for (int i=1; i < packet_size-1; i++) if (isX5) sum += data[i]; else sum ^= data[i]; return sum + (isX5 ? 0 : 0x55); } #define BABS(X) (((X) < 0) ? -(unsigned char)(X) : (X)) // Channel values are sign + magnitude 8bit values unsigned char convert_channel(unsigned char num) { long ch = Channels[num]; if (ch < CHAN_MIN_VALUE) { ch = CHAN_MIN_VALUE; } else if (ch > CHAN_MAX_VALUE) { ch = CHAN_MAX_VALUE; } return (unsigned char) ((ch < 0 ? 0x80 : 0) | BABS(ch * 127 / CHAN_MAX_VALUE)); } void read_controls(unsigned char* throttle, unsigned char* rudder, unsigned char* elevator, unsigned char* aileron, unsigned char* flags) { // Protocol is registered AETRF, that is // Aileron is channel 1, Elevator - 2, Throttle - 3, Rudder - 4, Flip control - 5 *aileron = convert_channel(CHANNEL1);//roll *elevator = convert_channel(CHANNEL2);//pitch *throttle = convert_channel(CHANNEL3); *throttle = *throttle & 0x80 ? 0xff - *throttle : 0x80 + *throttle; *rudder = convert_channel(CHANNEL4);//heading // Channel 5 if (Channels[CHANNEL5] <= 0) *flags &= ~FLAG_FLIP; else *flags |= FLAG_FLIP; // Channel 6 if (Channels[CHANNEL6] <= 0) *flags &= ~FLAG_PICTURE; else *flags |= FLAG_PICTURE; // Channel 7 if (Channels[CHANNEL7] <= 0) *flags &= ~FLAG_VIDEO; else *flags |= FLAG_VIDEO; // dbgprintf("ail %5d, ele %5d, thr %5d, rud %5d, flags 0x%x\n", // *aileron, *elevator, *throttle, *rudder, *flags); } #define X5C_CHAN2TRIM(X) ((((X) & 0x80 ? 0xff - (X) : 0x80 + (X)) >> 2) + 0x20) void build_packet_x5c(unsigned char bind) { if (bind) { memset(packet, 0, packet_size); packet[7] = 0xae; packet[8] = 0xa9; packet[14] = 0xc0; packet[15] = 0x17; } else { read_controls(&throttle, &rudder, &elevator, &aileron, &flags); 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 & FLAG_VIDEO ? 0x10 : 0x00) | (flags & FLAG_PICTURE ? 0x08 : 0x00) | (flags & FLAG_FLIP ? 0x01 : 0x00) | 0x04; // always high rates (bit 3 is rate control) packet[15] = checksum(packet); } } void build_packet(unsigned char 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 { read_controls(&throttle, &rudder, &elevator, &aileron, &flags); packet[0] = throttle; packet[1] = elevator; packet[2] = rudder; packet[3] = aileron; packet[4] = (flags & FLAG_VIDEO ? 0x80 : 0x00) | (flags & FLAG_PICTURE ? 0x40 : 0x00); // use trims to extend controls packet[5] = (elevator >> 2) | 0xc0; // always high rates (bit 7 is rate control) packet[6] = (rudder >> 2) | (flags & FLAG_FLIP ? 0x40 : 0x00); packet[7] = aileron >> 2; packet[8] = 0x00; } packet[9] = checksum(packet); } void send_packet(unsigned char bind) { //Serial.println("send_packet"); if (isX5) build_packet_x5c(bind); else build_packet(bind); // clear packet status bits and TX FIFO radio.write_register(NRF24L01_07_STATUS, 0x70); radio.write_register(NRF24L01_00_CONFIG, 0x2e); radio.write_register(NRF24L01_05_RF_CH, chans[current_chan]); radio.flush_tx();//NRF24L01_FlushTx(); //Serial.print("current:"); //Serial.print(current_chan); //Serial.print(" Chan:"); //Serial.println(chans[current_chan]); //printf("current channel = %d = %x \n ",current_chan,chans[current_chan]); radio.write_payload(packet, packet_size);//NRF24L01_WritePayload(packet, packet_size); #ifdef EMULATOR dbgprintf("pkt %d: chan 0x%x, bind %d, data %02x", packet_counter, chans[current_chan], bind, packet[0]); for(int i=1; i < packet_size; i++) dbgprintf(" %02x", packet[i]); dbgprintf("\n"); #endif if (packet_counter++ % 2) { // use each channel twice current_chan = (current_chan + 1) % num_rf_channels; } // Check and adjust transmission power. We do this after // transmission to not bother with timeout after power // settings change - we have plenty of time until next // packet. radio.setPALevel(tx_power); /* if (tx_power != Model.tx_power) { //Keep transmit power updated tx_power = Model.tx_power; NRF24L01_SetPower(tx_power); } */ } // Generate address to use from TX id and manufacturer id (STM32 unique id) static void initialize_rx_tx_addr() { unsigned long lfsr = 0xb2c54a2ful; /* #ifndef USE_FIXED_MFGID unsigned char var[12]; MCU_SerialNumber(var, 12); dbgprintf("Manufacturer id: "); for (int i = 0; i < 12; ++i) { dbgprintf("%02X", var[i]); rand32_r(&lfsr, var[i]); } dbgprintf("\r\n"); #endif */ /* if (Model.fixed_id) { for (unsigned char i = 0, j = 0; i < sizeof(Model.fixed_id); ++i, j += 8) rand32_r(&lfsr, (Model.fixed_id >> j) & 0xff); } */ // Pump zero bytes for LFSR to diverge more for (unsigned char i = 0; i < sizeof(lfsr); ++i) rand32_r(&lfsr, 0); Serial.println("initialize_rx_tx_addr "); rx_tx_addr[4] = 0xa2; unsigned char i =0; for (i = 0; i < sizeof(rx_tx_addr)-1; ++i) { rx_tx_addr[i] = lfsr & 0xff; rand32_r(&lfsr, i); Serial.println(rx_tx_addr[i],HEX); } Serial.println(rx_tx_addr[i],HEX); } void my_syma_init() { const unsigned char bind_rx_tx_addr[] = {0xab,0xac,0xad,0xae,0xaf}; const unsigned char rx_tx_addr_x5c[] = {0x6d,0x6a,0x73,0x73,0x73}; // X5C uses same address for bind and data radio.begin(); radio.setAutoAck(false); //?? radio.openReadingPipe//??? radio.setRetries(0x04,0x0f); radio.setChannel(0x08); radio.setDataRate(RF24_250KBPS); packet_size = 10; radio.setPALevel(tx_power); } void symax_init() { const unsigned char bind_rx_tx_addr[] = {0xab,0xac,0xad,0xae,0xaf}; const unsigned char rx_tx_addr_x5c[] = {0x6d,0x6a,0x73,0x73,0x73}; // X5C uses same address for bind and data Serial.println("symax_init"); //NRF24L01_Initialize(); radio.begin();//?????????? NRF24L01_SetTxRxMode(TX_EN); radio.read_register(STATUS);// NRF24L01_ReadReg(NRF24L01_07_STATUS); // radio.write_register radio.write_register(NRF24L01_00_CONFIG, BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO)); radio.write_register(NRF24L01_01_EN_AA, 0x00); // No Auto Acknoledgement radio.write_register(NRF24L01_02_EN_RXADDR, 0x3F); // Enable all data pipes (even though not used?) radio.write_register(NRF24L01_03_SETUP_AW, 0x03); // 5-byte RX/TX address radio.write_register(NRF24L01_04_SETUP_RETR, 0xff); // 4mS retransmit t/o, 15 tries (retries w/o AA?) radio.write_register(NRF24L01_05_RF_CH, 0x08); if (isX5) { radio.setDataRate(RF24_1MBPS); //NRF24L01_SetBitrate(NRF24L01_BR_1M); packet_size = 16; } else { radio.setDataRate(RF24_250KBPS); //NRF24L01_SetBitrate(NRF24L01_BR_250K); packet_size = 10; } radio.setPALevel(tx_power); //NRF24L01_SetPower(Model.tx_power);//setPALevel differents units need some adapter or change radio.write_register(NRF24L01_07_STATUS, 0x70); // Clear data ready, data sent, and retransmit radio.write_register(NRF24L01_08_OBSERVE_TX, 0x00); radio.write_register(NRF24L01_09_CD, 0x00); radio.write_register(NRF24L01_0C_RX_ADDR_P2, 0xC3); // LSB byte of pipe 2 receive address radio.write_register(NRF24L01_0D_RX_ADDR_P3, 0xC4); radio.write_register(NRF24L01_0E_RX_ADDR_P4, 0xC5); radio.write_register(NRF24L01_0F_RX_ADDR_P5, 0xC6); radio.write_register(NRF24L01_11_RX_PW_P0, PAYLOADSIZE); // bytes of data payload for pipe 1 radio.write_register(NRF24L01_12_RX_PW_P1, PAYLOADSIZE); radio.write_register(NRF24L01_13_RX_PW_P2, PAYLOADSIZE); radio.write_register(NRF24L01_14_RX_PW_P3, PAYLOADSIZE); radio.write_register(NRF24L01_15_RX_PW_P4, PAYLOADSIZE); radio.write_register(NRF24L01_16_RX_PW_P5, PAYLOADSIZE); radio.write_register(NRF24L01_17_FIFO_STATUS, 0x00); // Just in case, no real bits to write here radio.write_register(NRF24L01_10_TX_ADDR,isX5 ? rx_tx_addr_x5c : bind_rx_tx_addr, 5);//Multi radio.read_register(STATUS); //NRF24L01_ReadReg(NRF24L01_07_STATUS); // Check for Beken BK2421/BK2423 chip // It is done by using Beken specific activate code, 0x53 // and checking that status register changed appropriately // There is no harm to run it on nRF24L01 because following // closing activate command changes state back even if it // does something on nRF24L01 radio.activate(0x53);// NRF24L01_Activate(0x53); // magic for BK2421 bank switch //toggle_features need to define a new function with parameter dbgprintf("Trying to switch banks\n"); //if (NRF24L01_ReadReg(NRF24L01_07_STATUS) & 0x80) { if (radio.read_register(NRF24L01_07_STATUS) & 0x80) { dbgprintf("BK2421 detected\n"); // Beken registers don't have such nice names, so we just mention // them by their numbers // It's all magic, eavesdropped from real transfer and not even from the // data sheet - it has slightly different values radio.write_register(0x00, (unsigned char *) "\x40\x4B\x01\xE2", 4); radio.write_register(0x01, (unsigned char *) "\xC0\x4B\x00\x00", 4); radio.write_register(0x02, (unsigned char *) "\xD0\xFC\x8C\x02", 4); radio.write_register(0x03, (unsigned char *) "\x99\x00\x39\x21", 4); radio.write_register(0x04, (unsigned char *) "\xF9\x96\x82\x1B", 4); radio.write_register(0x05, (unsigned char *) "\x24\x06\x7F\xA6", 4); radio.write_register(0x06, (unsigned char *) "\x00\x00\x00\x00", 4); radio.write_register(0x07, (unsigned char *) "\x00\x00\x00\x00", 4); radio.write_register(0x08, (unsigned char *) "\x00\x00\x00\x00", 4); radio.write_register(0x09, (unsigned char *) "\x00\x00\x00\x00", 4); radio.write_register(0x0A, (unsigned char *) "\x00\x00\x00\x00", 4); radio.write_register(0x0B, (unsigned char *) "\x00\x00\x00\x00", 4); radio.write_register(0x0C, (unsigned char *) "\x00\x12\x73\x00", 4); radio.write_register(0x0D, (unsigned char *) "\x46\xB4\x80\x00", 4); radio.write_register(0x0E, (unsigned char *) "\x41\x10\x04\x82\x20\x08\x08\xF2\x7D\xEF\xFF", 11); radio.write_register(0x04, (unsigned char *) "\xFF\x96\x82\x1B", 4); radio.write_register(0x04, (unsigned char *) "\xF9\x96\x82\x1B", 4); } else { dbgprintf("nRF24L01 detected\n"); } radio.activate(0x53);// NRF24L01_Activate(0x53); // switch bank back // NRF24L01_FlushTx(); radio.flush_tx(); radio.read_register(STATUS);// NRF24L01_ReadReg(NRF24L01_07_STATUS); radio.write_register(NRF24L01_07_STATUS, 0x0e); radio.read_register(NRF24L01_00_CONFIG); // NRF24L01_ReadReg(NRF24L01_00_CONFIG); radio.write_register(NRF24L01_00_CONFIG, 0x0c); radio.write_register(NRF24L01_00_CONFIG, 0x0e); // power on } void symax_init1() { // write a strange first packet to RF channel 8 ... unsigned char first_packet[] = {0xf9, 0x96, 0x82, 0x1b, 0x20, 0x08, 0x08, 0xf2, 0x7d, 0xef, 0xff, 0x00, 0x00, 0x00, 0x00}; unsigned char chans_bind[] = {0x4b, 0x30, 0x40, 0x2e}; unsigned char chans_bind_x5c[] = {0x27, 0x1b, 0x39, 0x28, 0x24, 0x22, 0x2e, 0x36, 0x19, 0x21, 0x29, 0x14, 0x1e, 0x12, 0x2d, 0x18}; Serial.println("symax_init1"); radio.flush_tx();// NRF24L01_FlushTx(); radio.write_register(NRF24L01_05_RF_CH, 0x08); radio.write_payload(first_packet, 15);//NRF24L01_WritePayload(first_packet, 15); if (isX5) { num_rf_channels = sizeof(chans_bind_x5c); memcpy(chans, chans_bind_x5c, num_rf_channels); } else { initialize_rx_tx_addr(); // make info available for bind packets num_rf_channels = sizeof(chans_bind);// i think 4 memcpy(chans, chans_bind, num_rf_channels); } current_chan = 0; packet_counter = 0; } // channels determined by last byte of tx address void set_channels(unsigned char address) { static const unsigned char start_chans_1[] = {0x0a, 0x1a, 0x2a, 0x3a}; static const unsigned char start_chans_2[] = {0x2a, 0x0a, 0x42, 0x22};//<<---- static const unsigned char start_chans_3[] = {0x1a, 0x3a, 0x12, 0x32}; unsigned char laddress = address & 0x1f; unsigned char i; unsigned long *pchans = (unsigned long *)chans; // avoid compiler warning num_rf_channels = 4; Serial.print("laddress:"); Serial.println(laddress); if (laddress < 0x10) { if (laddress == 6) laddress = 7; for(i=0; i < num_rf_channels; i++) { chans[i] = start_chans_1[i] + laddress; } } else if (laddress < 0x18) { for(i=0; i < num_rf_channels; i++) { chans[i] = start_chans_2[i] + (laddress & 0x07); } if (laddress == 0x16) { chans[0] += 1; chans[1] += 1; } } else if (laddress < 0x1e) { for(i=0; i < num_rf_channels; i++) { chans[i] = start_chans_3[i] + (laddress & 0x07); } } else if (laddress == 0x1e) { *pchans = 0x38184121; } else { *pchans = 0x39194121; } } void symax_init2() { unsigned char chans_data_x5c[] = {0x1d, 0x2f, 0x26, 0x3d, 0x15, 0x2b, 0x25, 0x24, 0x27, 0x2c, 0x1c, 0x3e, 0x39, 0x2d, 0x22}; Serial.println("symax_init2"); if (isX5) { num_rf_channels = sizeof(chans_data_x5c); memcpy(chans, chans_data_x5c, num_rf_channels); } else { set_channels(rx_tx_addr[0]); radio.write_register(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);// } current_chan = 0; packet_counter = 0; } unsigned long t; long symax_callback() { switch (phase) { case SYMAX_INIT1: symax_init1(); phase = SYMAX_BIND2; btime = micros(); return FIRST_PACKET_DELAY; break; case SYMAX_BIND2: counter = BIND_COUNT; phase = SYMAX_BIND3; Serial.println("SYMAX_BIND2"); send_packet(1); break; case SYMAX_BIND3: Serial.println("SYMAX_BIND3"); t = micros()-btime; if (counter == 0){//counter == 0) {//t> 1400 symax_init2(); phase = SYMAX_DATA; //Serial.print("time:"); //Serial.println(micros()-btime); //PROTOCOL_SetBindState(0); //MUSIC_Play(MUSIC_DONE_BINDING); } else { //Serial.print("counter"); //Serial.println(counter); send_packet(1); counter -= 1; } break; case SYMAX_DATA: //Serial.println("SYMAX_DATA"); send_packet(0); break; } return PACKET_PERIOD; } void initialize() { //CLOCK_StopTimer(); tx_power = RF24_PA_MAX;//Model.tx_power; packet_counter = 0; flags = 0; symax_init(); phase = SYMAX_INIT1; //PROTOCOL_SetBindState(BIND_COUNT * PACKET_PERIOD / 1000); // CLOCK_StartTimer(INITIAL_WAIT, symax_callback); period = INITIAL_WAIT; } void timerIsr() { new_period = symax_callback(); Timer1.setPeriod(new_period); } // void SYMAX_Cmds(ProtoCmds cmd) //{ // switch(cmd) { // case PROTOCMD_INIT: initialize(); return 0; // case PROTOCMD_DEINIT: // case PROTOCMD_RESET: /// CLOCK_StopTimer(); // return (void *)(NRF24L01_Reset() ? 1L : -1L); // case PROTOCMD_CHECK_AUTOBIND: return (void *)1L; // always Autobind // case PROTOCMD_BIND: initialize(); return 0; // case PROTOCMD_NUMCHAN: return (void *) 8L; // A, E, T, R, special controls // case PROTOCMD_DEFAULT_NUMCHAN: return (void *)5L; // case PROTOCMD_CURRENT_ID: return Model.fixed_id ? (void *)((unsigned long)Model.fixed_id) : 0; //case PROTOCMD_GETOPTIONS: return symax_opts; //case PROTOCMD_TELEMETRYSTATE: return (void *)(long)PROTO_TELEM_UNSUPPORTED; // default: break; // } // return 0; //} int interval = 0; boolean binding = true; void setup() { // put your setup code here, to run once: //radio.begin(); //randomSeed(analogRead(0)); Serial.begin(115200); printf_begin(); initialize(); btime = micros(); radio.printDetails(); Timer1.initialize(period); Timer1.attachInterrupt( timerIsr ); } void loop() { if (Serial.available()) { int input_value; Serial.print("received: "); for (int i = 0; i < NUM_ACTIVE_CHANNELS; i++) { input_value = Serial.parseInt(); input_value = input_value<0?0:input_value; input_value = input_value>CHANNEL_DSC?CHANNEL_DSC:input_value; Channels[i] = input_value; Serial.print(input_value, DEC); Serial.print(i