gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-04 21:08:12 +00:00
Code Issues Projects Releases Wiki Activity

Devo fix

Browse Source
This commit is contained in:
pascallanger 2016-08-25 13:42:21 +02:00
parent c2b9376313
commit e7ed80d3e0
1 changed files with 115 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

317
Multiprotocol/Devo_cyrf6936.ino
View File

@ -21,16 +21,13 @@
//For Debug //For Debug
//#define NO_SCRAMBLE //#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,
DEVO_BIND_SENDCH, DEVO_BIND_SENDCH,
DEVO_BOUND, DEVO_BOUND,
@ -46,7 +43,7 @@ enum Devo_PhaseState
DEVO_BOUND_10, DEVO_BOUND_10,
}; };
const uint8_t sopcodes[][8] = { const uint8_t PROGMEM DEVO_sopcodes[][8] = {
/* Note these are in order transmitted (LSB 1st) */ /* Note these are in order transmitted (LSB 1st) */
/* 0 */ {0x3C,0x37,0xCC,0x91,0xE2,0xF8,0xCC,0x91}, //0x91CCF8E291CC373C /* 0 */ {0x3C,0x37,0xCC,0x91,0xE2,0xF8,0xCC,0x91}, //0x91CCF8E291CC373C
/* 1 */ {0x9B,0xC5,0xA1,0x0F,0xAD,0x39,0xA2,0x0F}, //0x0FA239AD0FA1C59B /* 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 /* 9 */ {0x97,0xE5,0x14,0x72,0x7F,0x1A,0x14,0x72}, //0x72141A7F7214E597
}; };
uint8_t txState; static void __attribute__((unused)) DEVO_ConfigSOPCode(uint8_t val)
uint8_t pkt_num; {
uint8_t ch_idx; uint8_t code[8];
uint8_t use_fixed_id; for(uint8_t i=0;i<8;i++)
uint8_t failsafe_pkt; 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 #ifdef NO_SCRAMBLE
return; return;
#else #else
uint8_t i; for(uint8_t i = 0; i < 15; i++)
for(i = 0; i < 15; i++)
packet[i + 1] ^= cyrfmfg_id[i % 4]; packet[i + 1] ^= cyrfmfg_id[i % 4];
#endif #endif
} }
static void __attribute__((unused)) add_pkt_suffix() static void __attribute__((unused)) DEVO_add_pkt_suffix()
{ {
uint8_t bind_state; uint8_t bind_state;
if (use_fixed_id) if (option)
{ {
if (bind_counter > 0) if (bind_counter > 0)
bind_state = 0xc0; bind_state = 0xc0;
else else
bind_state = 0x80; bind_state = 0x80;
} }
else else
bind_state = 0x00; bind_state = 0x00;
packet[10] = bind_state | (PKTS_PER_CHANNEL - pkt_num - 1); packet[10] = bind_state | (DEVO_PKTS_PER_CHANNEL - packet_count - 1);
packet[11] = *(hopping_frequency_ptr + 1); packet[11] = *(hopping_frequency_ptr + 1);
packet[12] = *(hopping_frequency_ptr + 2); packet[12] = *(hopping_frequency_ptr + 2);
packet[13] = fixed_id & 0xff; packet[13] = fixed_id & 0xff;
@ -97,59 +95,50 @@ static void __attribute__((unused)) add_pkt_suffix()
packet[15] = (fixed_id >> 16) & 0xff; 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); packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x07;
// uint8_t enable = 0;
uint8_t max = 8; uint8_t max = 8;
// int offset = 0;
if (upper) if (upper)
{ {
packet[0] += 1; packet[0] += 1;
max = 4; max = 4;
// offset = 8;
} }
for(uint8_t i = 0; i < max; i++) for(uint8_t i = 0; i < max; i++)
packet[i+1] = 0; packet[i+1] = 0;
// packet[9] = enable;
packet[9] = 0; packet[9] = 0;
add_pkt_suffix(); DEVO_add_pkt_suffix();
} }
#define FORCE_INDIRECT(ptr) __asm__ __volatile__ ("" : "=e" (ptr) : "0" (ptr)) static void __attribute__((unused)) DEVO_build_bind_pkt()
static void __attribute__((unused)) build_bind_pkt()
{ {
uint8_t *p = packet ; packet[0] = (DEVO_NUM_CHANNELS << 4) | 0x0a;
FORCE_INDIRECT(p) ; packet[1] = bind_counter & 0xff;
p[0] = (DEVO_NUM_CHANNELS << 4) | 0x0a; packet[2] = (bind_counter >> 8);
p[1] = bind_counter & 0xff; packet[3] = *hopping_frequency_ptr;
p[2] = (bind_counter >> 8); packet[4] = *(hopping_frequency_ptr + 1);
p[3] = *hopping_frequency_ptr; packet[5] = *(hopping_frequency_ptr + 2);
p[4] = *(hopping_frequency_ptr + 1); packet[6] = cyrfmfg_id[0];
p[5] = *(hopping_frequency_ptr + 2); packet[7] = cyrfmfg_id[1];
p[6] = cyrfmfg_id[0]; packet[8] = cyrfmfg_id[2];
p[7] = cyrfmfg_id[1]; packet[9] = cyrfmfg_id[3];
p[8] = cyrfmfg_id[2]; DEVO_add_pkt_suffix();
p[9] = cyrfmfg_id[3];
add_pkt_suffix();
//The fixed-id portion is scrambled in the bind packet //The fixed-id portion is scrambled in the bind packet
//I assume it is ignored //I assume it is ignored
p[13] ^= cyrfmfg_id[0]; packet[13] ^= cyrfmfg_id[0];
p[14] ^= cyrfmfg_id[1]; packet[14] ^= cyrfmfg_id[1];
p[15] ^= cyrfmfg_id[2]; 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); packet[0] = (DEVO_NUM_CHANNELS << 4) | (0x0b + ch_idx);
uint8_t sign = 0x0b; 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
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
if(value < 0) if(value < 0)
{ {
value = -value; value = -value;
@ -159,13 +148,13 @@ static void __attribute__((unused)) build_data_pkt()
packet[2 * i + 2] = (value >> 8) & 0xff; packet[2 * i + 2] = (value >> 8) & 0xff;
} }
packet[9] = sign; packet[9] = sign;
ch_idx = ch_idx + 1; ch_idx++;
if (ch_idx * 4 >= DEVO_NUM_CHANNELS) if (ch_idx * 4 >= DEVO_NUM_CHANNELS)
ch_idx = 0; 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 */ /* 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]); 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; uint8_t sopidx = (0xff &((cyrfmfg_id[0] << 2) + cyrfmfg_id[1] + cyrfmfg_id[2])) % 10;
CYRF_SetTxRxMode(TX_EN); CYRF_SetTxRxMode(TX_EN);
CYRF_ConfigCRCSeed((crc << 8) + crc); CYRF_ConfigCRCSeed((crc << 8) + crc);
CYRF_ConfigSOPCode(sopcodes[sopidx]); DEVO_ConfigSOPCode(sopidx);
CYRF_SetPower(0x08); CYRF_SetPower(0x08);
} }
const uint8_t PROGMEM devo_init_vals[][2] = { const uint8_t PROGMEM DEVO_init_vals[][2] = {
{CYRF_06_RX_CFG, 0x4A }, { CYRF_1D_MODE_OVERRIDE, 0x38 },
{CYRF_0B_PWR_CTRL, 0x00 }, { CYRF_03_TX_CFG, 0x08 },
{CYRF_0D_IO_CFG, 0x04 }, { CYRF_06_RX_CFG, 0x4A },
{CYRF_0E_GPIO_CTRL, 0x20 }, { CYRF_0B_PWR_CTRL, 0x00 },
{CYRF_10_FRAMING_CFG, 0xA4 }, { CYRF_10_FRAMING_CFG, 0xA4 },
{CYRF_11_DATA32_THOLD, 0x05 }, { CYRF_11_DATA32_THOLD, 0x05 },
{CYRF_12_DATA64_THOLD, 0x0E }, { CYRF_12_DATA64_THOLD, 0x0E },
{CYRF_1B_TX_OFFSET_LSB, 0x55 }, { CYRF_1B_TX_OFFSET_LSB, 0x55 },
{CYRF_1C_TX_OFFSET_MSB, 0x05 }, { CYRF_1C_TX_OFFSET_MSB, 0x05 },
{CYRF_32_AUTO_CAL_TIME, 0x3C }, { CYRF_32_AUTO_CAL_TIME, 0x3C },
{CYRF_35_AUTOCAL_OFFSET, 0x14 }, { CYRF_35_AUTOCAL_OFFSET, 0x14 },
{CYRF_39_ANALOG_CTRL, 0x01 }, { CYRF_39_ANALOG_CTRL, 0x01 },
{CYRF_1E_RX_OVERRIDE, 0x10 }, { CYRF_1E_RX_OVERRIDE, 0x10 },
{CYRF_1F_TX_OVERRIDE, 0x00 }, { CYRF_1F_TX_OVERRIDE, 0x00 },
{CYRF_01_TX_LENGTH, 0x10 }, { CYRF_01_TX_LENGTH, 0x10 },
{CYRF_0C_XTAL_CTRL, 0xC0 }, { CYRF_0F_XACT_CFG, 0x10 },
{CYRF_0F_XACT_CFG, 0x10 }, { CYRF_27_CLK_OVERRIDE, 0x02 },
{CYRF_27_CLK_OVERRIDE, 0x02 }, { CYRF_28_CLK_EN, 0x02 },
{CYRF_28_CLK_EN, 0x02 }, { CYRF_0F_XACT_CFG, 0x28 }
{CYRF_0F_XACT_CFG, 0x28 }
}; };
static void __attribute__((unused)) cyrf_init() static void __attribute__((unused)) DEVO_cyrf_init()
{ {
/* Initialise CYRF chip */ /* Initialise CYRF chip */
CYRF_WriteRegister(CYRF_1D_MODE_OVERRIDE, 0x39); for(uint8_t i = 0; i < sizeof(DEVO_init_vals) / 2; i++)
CYRF_SetPower(0x08); CYRF_WriteRegister(pgm_read_byte( &DEVO_init_vals[i][0]), pgm_read_byte( &DEVO_init_vals[i][1]) );
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);
} }
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); 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[3] = hopping_frequency[0];
hopping_frequency[4] = hopping_frequency[1]; hopping_frequency[4] = hopping_frequency[1];
} }
static void __attribute__((unused)) DEVO_BuildPacket() static void __attribute__((unused)) DEVO_BuildPacket()
{ {
static uint8_t failsafe_pkt=0;
switch(phase) switch(phase)
{ {
case DEVO_BIND: case DEVO_BIND:
if(bind_counter>0) if(bind_counter)
bind_counter--; bind_counter--;
build_bind_pkt(); DEVO_build_bind_pkt();
phase = DEVO_BIND_SENDCH; phase = DEVO_BIND_SENDCH;
break; break;
case DEVO_BIND_SENDCH: case DEVO_BIND_SENDCH:
if(bind_counter>0) if(bind_counter)
bind_counter--; bind_counter--;
build_data_pkt(); DEVO_build_data_pkt();
scramble_pkt(); DEVO_scramble_pkt();
if (bind_counter == 0) if (bind_counter == 0)
{ {
phase = DEVO_BOUND; phase = DEVO_BOUND;
@ -280,10 +237,10 @@ static void __attribute__((unused)) DEVO_BuildPacket()
case DEVO_BOUND_7: case DEVO_BOUND_7:
case DEVO_BOUND_8: case DEVO_BOUND_8:
case DEVO_BOUND_9: case DEVO_BOUND_9:
build_data_pkt(); DEVO_build_data_pkt();
scramble_pkt(); DEVO_scramble_pkt();
phase++; phase++;
if (bind_counter > 0) if (bind_counter)
{ {
bind_counter--; bind_counter--;
if (bind_counter == 0) if (bind_counter == 0)
@ -291,19 +248,20 @@ static void __attribute__((unused)) DEVO_BuildPacket()
} }
break; break;
case DEVO_BOUND_10: 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; failsafe_pkt = failsafe_pkt ? 0 : 1;
scramble_pkt(); DEVO_scramble_pkt();
phase = DEVO_BOUND_1; phase = DEVO_BOUND_1;
break; break;
} }
pkt_num++; packet_count++;
if(pkt_num == PKTS_PER_CHANNEL) if(packet_count == DEVO_PKTS_PER_CHANNEL)
pkt_num = 0; packet_count = 0;
} }
uint16_t devo_callback() uint16_t devo_callback()
{ {
static uint8_t txState=0;
if (txState == 0) if (txState == 0)
{ {
txState = 1; txState = 1;
@ -314,107 +272,58 @@ uint16_t devo_callback()
txState = 0; txState = 0;
uint8_t i = 0; uint8_t i = 0;
while (! (CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02)) while (! (CYRF_ReadRegister(CYRF_04_TX_IRQ_STATUS) & 0x02))
if(++i > NUM_WAIT_LOOPS) if(++i > DEVO_NUM_WAIT_LOOPS)
return 1200; return 1200;
if (phase == DEVO_BOUND) if (phase == DEVO_BOUND)
{ {
/* exit binding state */ /* exit binding state */
phase = DEVO_BOUND_3; 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); //Keep tx power updated
CYRF_SetPower(0x08);
hopping_frequency_ptr = hopping_frequency_ptr == &hopping_frequency[2] ? hopping_frequency : hopping_frequency_ptr + 1; hopping_frequency_ptr = hopping_frequency_ptr == &hopping_frequency[2] ? hopping_frequency : hopping_frequency_ptr + 1;
CYRF_ConfigRFChannel(*hopping_frequency_ptr); CYRF_ConfigRFChannel(*hopping_frequency_ptr);
} }
return 1200; 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() uint16_t DevoInit()
{ {
CYRF_Reset(); DEVO_cyrf_init();
cyrf_init();
CYRF_GetMfgData(cyrfmfg_id); CYRF_GetMfgData(cyrfmfg_id);
CYRF_SetTxRxMode(TX_EN); CYRF_SetTxRxMode(TX_EN);
CYRF_ConfigCRCSeed(0x0000); CYRF_ConfigCRCSeed(0x0000);
CYRF_ConfigSOPCode(sopcodes[0]); DEVO_ConfigSOPCode(0);
set_radio_channels(); DEVO_set_radio_channels();
use_fixed_id = 0;
failsafe_pkt = 0;
hopping_frequency_ptr = hopping_frequency; hopping_frequency_ptr = hopping_frequency;
//
CYRF_ConfigRFChannel(*hopping_frequency_ptr); CYRF_ConfigRFChannel(*hopping_frequency_ptr);
//FIXME: Properly setnumber of channels;
pkt_num = 0;
ch_idx = 0;
txState = 0;
//uint8_t txid[4];
//
/* packet_count = 0;
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) if(option==0)
{//model fixed ID =0 {
fixed_id = ((uint32_t)(hopping_frequency[0] ^ cyrfmfg_id[0] ^ cyrfmfg_id[3]) << 16) 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[1] ^ cyrfmfg_id[1] ^ cyrfmfg_id[4]) << 8)
| ((uint32_t)(hopping_frequency[2] ^ cyrfmfg_id[2] ^ cyrfmfg_id[5]) << 0); | ((uint32_t)(hopping_frequency[2] ^ cyrfmfg_id[2] ^ cyrfmfg_id[5]) << 0);
fixed_id = fixed_id % 1000000;
bind_counter = DEVO_BIND_COUNT; bind_counter = DEVO_BIND_COUNT;
phase = DEVO_BIND; phase = DEVO_BIND;
//PROTOCOL_SetBindState(0x1388 * 2400 / 1000); //msecs BIND_IN_PROGRESS;
} }
else else
{ {
fixed_id = Model_fixed_id;
use_fixed_id = 1;
phase = DEVO_BOUND_1; phase = DEVO_BOUND_1;
bind_counter = 0; 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; return 2400;
} }