From e7ed80d3e0efe78590bfc1b75f20895e1ca40c9e Mon Sep 17 00:00:00 2001 From: pascallanger Date: Thu, 25 Aug 2016 13:42:21 +0200 Subject: [PATCH] Devo fix --- Multiprotocol/Devo_cyrf6936.ino | 321 ++++++++++++-------------------- 1 file changed, 115 insertions(+), 206 deletions(-) diff --git a/Multiprotocol/Devo_cyrf6936.ino b/Multiprotocol/Devo_cyrf6936.ino index e535095..d2ed525 100644 --- a/Multiprotocol/Devo_cyrf6936.ino +++ b/Multiprotocol/Devo_cyrf6936.ino @@ -21,16 +21,13 @@ //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 -{ +#define DEVO_PKTS_PER_CHANNEL 4 +#define DEVO_BIND_COUNT 0x1388 + +#define DEVO_NUM_WAIT_LOOPS (100 / 5) //each loop is ~5us. Do not wait more than 100us + +enum { DEVO_BIND, DEVO_BIND_SENDCH, DEVO_BOUND, @@ -46,7 +43,7 @@ enum Devo_PhaseState DEVO_BOUND_10, }; -const uint8_t sopcodes[][8] = { +const uint8_t PROGMEM DEVO_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 @@ -60,36 +57,37 @@ const uint8_t sopcodes[][8] = { /* 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; +static void __attribute__((unused)) DEVO_ConfigSOPCode(uint8_t val) +{ + uint8_t code[8]; + for(uint8_t i=0;i<8;i++) + code[i]=pgm_read_byte_near(&DEVO_sopcodes[val][i]); + CYRF_ConfigSOPCode(code); +} -static void __attribute__((unused)) scramble_pkt() +static void __attribute__((unused)) DEVO_scramble_pkt() { #ifdef NO_SCRAMBLE return; #else - uint8_t i; - for(i = 0; i < 15; i++) + for(uint8_t i = 0; i < 15; i++) packet[i + 1] ^= cyrfmfg_id[i % 4]; #endif } -static void __attribute__((unused)) add_pkt_suffix() +static void __attribute__((unused)) DEVO_add_pkt_suffix() { - uint8_t bind_state; - if (use_fixed_id) + uint8_t bind_state; + if (option) { - if (bind_counter > 0) - bind_state = 0xc0; - else - bind_state = 0x80; - } + 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); + bind_state = 0x00; + packet[10] = bind_state | (DEVO_PKTS_PER_CHANNEL - packet_count - 1); packet[11] = *(hopping_frequency_ptr + 1); packet[12] = *(hopping_frequency_ptr + 2); packet[13] = fixed_id & 0xff; @@ -97,59 +95,50 @@ static void __attribute__((unused)) add_pkt_suffix() packet[15] = (fixed_id >> 16) & 0xff; } -static void __attribute__((unused)) build_beacon_pkt(uint8_t upper) +static void __attribute__((unused)) DEVO_build_beacon_pkt(uint8_t upper) { - packet[0] = ((DEVO_NUM_CHANNELS << 4) | 0x07); -// uint8_t enable = 0; + packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x07; 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(); + DEVO_add_pkt_suffix(); } -#define FORCE_INDIRECT(ptr) __asm__ __volatile__ ("" : "=e" (ptr) : "0" (ptr)) - -static void __attribute__((unused)) build_bind_pkt() +static void __attribute__((unused)) DEVO_build_bind_pkt() { - uint8_t *p = packet ; - FORCE_INDIRECT(p) ; - p[0] = (DEVO_NUM_CHANNELS << 4) | 0x0a; - p[1] = bind_counter & 0xff; - p[2] = (bind_counter >> 8); - p[3] = *hopping_frequency_ptr; - p[4] = *(hopping_frequency_ptr + 1); - p[5] = *(hopping_frequency_ptr + 2); - p[6] = cyrfmfg_id[0]; - p[7] = cyrfmfg_id[1]; - p[8] = cyrfmfg_id[2]; - p[9] = cyrfmfg_id[3]; - add_pkt_suffix(); + 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]; + DEVO_add_pkt_suffix(); //The fixed-id portion is scrambled in the bind packet //I assume it is ignored - p[13] ^= cyrfmfg_id[0]; - p[14] ^= cyrfmfg_id[1]; - p[15] ^= cyrfmfg_id[2]; + packet[13] ^= cyrfmfg_id[0]; + packet[14] ^= cyrfmfg_id[1]; + packet[15] ^= cyrfmfg_id[2]; } -static void __attribute__((unused)) build_data_pkt() +static void __attribute__((unused)) DEVO_build_data_pkt() { - uint8_t i; + static uint8_t ch_idx=0; + packet[0] = (DEVO_NUM_CHANNELS << 4) | (0x0b + ch_idx); uint8_t sign = 0x0b; - for (i = 0; i < 4; i++) + for (uint8_t i = 0; i < 4; i++) { - // - int16_t value= map(Servo_data[ch_idx * 4 + i],servo_min_125,servo_max_125,-1600,1600);//range -1600...+1600 - //s32 value = (s32)Channels[ch_idx * 4 + i] * 0x640 / CHAN_MAX_VALUE;//10000 + int16_t value=map(Servo_data[ch_idx * 4 + i],servo_min_125,servo_max_125,-1600,1600);//range -1600..+1600 if(value < 0) { value = -value; @@ -159,13 +148,13 @@ static void __attribute__((unused)) build_data_pkt() packet[2 * i + 2] = (value >> 8) & 0xff; } packet[9] = sign; - ch_idx = ch_idx + 1; + ch_idx++; if (ch_idx * 4 >= DEVO_NUM_CHANNELS) ch_idx = 0; - add_pkt_suffix(); + DEVO_add_pkt_suffix(); } -static void __attribute__((unused)) cyrf_set_bound_sop_code() +static void __attribute__((unused)) DEVO_cyrf_set_bound_sop_code() { /* crc == 0 isn't allowed, so use 1 if the math results in 0 */ uint8_t crc = (cyrfmfg_id[0] + (cyrfmfg_id[1] >> 6) + cyrfmfg_id[2]); @@ -174,94 +163,62 @@ static void __attribute__((unused)) cyrf_set_bound_sop_code() 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]); + DEVO_ConfigSOPCode(sopidx); CYRF_SetPower(0x08); } -const uint8_t PROGMEM devo_init_vals[][2] = { - {CYRF_06_RX_CFG, 0x4A }, - {CYRF_0B_PWR_CTRL, 0x00 }, - {CYRF_0D_IO_CFG, 0x04 }, - {CYRF_0E_GPIO_CTRL, 0x20 }, - {CYRF_10_FRAMING_CFG, 0xA4 }, - {CYRF_11_DATA32_THOLD, 0x05 }, - {CYRF_12_DATA64_THOLD, 0x0E }, - {CYRF_1B_TX_OFFSET_LSB, 0x55 }, - {CYRF_1C_TX_OFFSET_MSB, 0x05 }, - {CYRF_32_AUTO_CAL_TIME, 0x3C }, - {CYRF_35_AUTOCAL_OFFSET, 0x14 }, - {CYRF_39_ANALOG_CTRL, 0x01 }, - {CYRF_1E_RX_OVERRIDE, 0x10 }, - {CYRF_1F_TX_OVERRIDE, 0x00 }, - {CYRF_01_TX_LENGTH, 0x10 }, - {CYRF_0C_XTAL_CTRL, 0xC0 }, - {CYRF_0F_XACT_CFG, 0x10 }, - {CYRF_27_CLK_OVERRIDE, 0x02 }, - {CYRF_28_CLK_EN, 0x02 }, - {CYRF_0F_XACT_CFG, 0x28 } +const uint8_t PROGMEM DEVO_init_vals[][2] = { + { CYRF_1D_MODE_OVERRIDE, 0x38 }, + { CYRF_03_TX_CFG, 0x08 }, + { CYRF_06_RX_CFG, 0x4A }, + { CYRF_0B_PWR_CTRL, 0x00 }, + { CYRF_10_FRAMING_CFG, 0xA4 }, + { CYRF_11_DATA32_THOLD, 0x05 }, + { CYRF_12_DATA64_THOLD, 0x0E }, + { CYRF_1B_TX_OFFSET_LSB, 0x55 }, + { CYRF_1C_TX_OFFSET_MSB, 0x05 }, + { CYRF_32_AUTO_CAL_TIME, 0x3C }, + { CYRF_35_AUTOCAL_OFFSET, 0x14 }, + { CYRF_39_ANALOG_CTRL, 0x01 }, + { CYRF_1E_RX_OVERRIDE, 0x10 }, + { CYRF_1F_TX_OVERRIDE, 0x00 }, + { CYRF_01_TX_LENGTH, 0x10 }, + { CYRF_0F_XACT_CFG, 0x10 }, + { CYRF_27_CLK_OVERRIDE, 0x02 }, + { CYRF_28_CLK_EN, 0x02 }, + { CYRF_0F_XACT_CFG, 0x28 } }; -static void __attribute__((unused)) cyrf_init() +static void __attribute__((unused)) DEVO_cyrf_init() { /* Initialise CYRF chip */ - CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x39); - CYRF_SetPower(0x08); - for(uint8_t i = 0; i < sizeof(devo_init_vals) / 2; i++) - CYRF_WriteRegister(pgm_read_byte( &devo_init_vals[i][0]), pgm_read_byte( &devo_init_vals[i][1]) ); - -// 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); + for(uint8_t i = 0; i < sizeof(DEVO_init_vals) / 2; i++) + CYRF_WriteRegister(pgm_read_byte( &DEVO_init_vals[i][0]), pgm_read_byte( &DEVO_init_vals[i][1]) ); } -static void __attribute__((unused)) set_radio_channels() +static void __attribute__((unused)) DEVO_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]; } static void __attribute__((unused)) DEVO_BuildPacket() { + static uint8_t failsafe_pkt=0; switch(phase) { case DEVO_BIND: - if(bind_counter>0) + if(bind_counter) bind_counter--; - build_bind_pkt(); + DEVO_build_bind_pkt(); phase = DEVO_BIND_SENDCH; break; case DEVO_BIND_SENDCH: - if(bind_counter>0) + if(bind_counter) bind_counter--; - build_data_pkt(); - scramble_pkt(); + DEVO_build_data_pkt(); + DEVO_scramble_pkt(); if (bind_counter == 0) { phase = DEVO_BOUND; @@ -280,10 +237,10 @@ static void __attribute__((unused)) DEVO_BuildPacket() case DEVO_BOUND_7: case DEVO_BOUND_8: case DEVO_BOUND_9: - build_data_pkt(); - scramble_pkt(); + DEVO_build_data_pkt(); + DEVO_scramble_pkt(); phase++; - if (bind_counter > 0) + if (bind_counter) { bind_counter--; if (bind_counter == 0) @@ -291,19 +248,20 @@ static void __attribute__((unused)) DEVO_BuildPacket() } break; case DEVO_BOUND_10: - build_beacon_pkt(DEVO_NUM_CHANNELS > 8 ? failsafe_pkt : 0); + DEVO_build_beacon_pkt(DEVO_NUM_CHANNELS > 8 ? failsafe_pkt : 0); failsafe_pkt = failsafe_pkt ? 0 : 1; - scramble_pkt(); + DEVO_scramble_pkt(); phase = DEVO_BOUND_1; break; } - pkt_num++; - if(pkt_num == PKTS_PER_CHANNEL) - pkt_num = 0; + packet_count++; + if(packet_count == DEVO_PKTS_PER_CHANNEL) + packet_count = 0; } uint16_t devo_callback() { + static uint8_t txState=0; if (txState == 0) { txState = 1; @@ -314,107 +272,58 @@ uint16_t devo_callback() txState = 0; uint8_t i = 0; while (! (CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02)) - if(++i > NUM_WAIT_LOOPS) + if(++i > DEVO_NUM_WAIT_LOOPS) return 1200; if (phase == DEVO_BOUND) { /* exit binding state */ phase = DEVO_BOUND_3; - cyrf_set_bound_sop_code(); + DEVO_cyrf_set_bound_sop_code(); } - if(pkt_num == 0) + if(packet_count == 0) { - //Keep tx power updated - CYRF_SetPower(0x08); + CYRF_SetPower(0x08); //Keep tx power updated hopping_frequency_ptr = hopping_frequency_ptr == &hopping_frequency[2] ? hopping_frequency : hopping_frequency_ptr + 1; CYRF_ConfigRFChannel(*hopping_frequency_ptr); } return 1200; } -/*static void __attribute__((unused)) devo_bind() -{ - fixed_id = Model_fixed_id; - bind_counter = DEVO_BIND_COUNT; - use_fixed_id = 1; - //PROTOCOL_SetBindState(0x1388 * 2400 / 1000); //msecs 12000ms -} - - -static void __attribute__((unused)) 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(); + DEVO_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; + DEVO_ConfigSOPCode(0); + DEVO_set_radio_channels(); + 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; -delayMilliseconds(100); -LED_OFF; -delayMilliseconds(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; + + packet_count = 0; + + if(option==0) + { + MProtocol_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); bind_counter = DEVO_BIND_COUNT; phase = DEVO_BIND; - //PROTOCOL_SetBindState(0x1388 * 2400 / 1000); //msecs + BIND_IN_PROGRESS; } else { - fixed_id = Model_fixed_id; - use_fixed_id = 1; phase = DEVO_BOUND_1; bind_counter = 0; - cyrf_set_bound_sop_code(); + DEVO_cyrf_set_bound_sop_code(); } + MProtocol_id %= 1000000; + if(IS_AUTOBIND_FLAG_on) + { + bind_counter = DEVO_BIND_COUNT; + } return 2400; }