mirror of
https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git
synced 2026-01-11 10:43:16 +00:00
Added FrSkyX protocol, Added MT99xx YZ sub protocol, Ram usage optimization
This commit is contained in:
pascallanger
@@ -1,21 +1,315 @@
|
||||
/*
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/* **************************
|
||||
* By Midelic on RCGroups *
|
||||
**************************
|
||||
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#if defined(FRSKYX_CC2500_INO)
|
||||
|
||||
#include "iface_cc2500.h"
|
||||
|
||||
uint8_t chanskip;
|
||||
uint8_t calData[48][3];
|
||||
uint8_t channr;
|
||||
uint8_t pass_ = 1 ;
|
||||
uint8_t counter_rst;
|
||||
uint8_t ctr;
|
||||
uint8_t FS_flag=0;
|
||||
// uint8_t ptr[4]={0x01,0x12,0x23,0x30};
|
||||
//uint8_t ptr[4]={0x00,0x11,0x22,0x33};
|
||||
|
||||
const PROGMEM uint8_t hop_data[]={
|
||||
0x02, 0xD4, 0xBB, 0xA2, 0x89,
|
||||
0x70, 0x57, 0x3E, 0x25, 0x0C,
|
||||
0xDE, 0xC5, 0xAC, 0x93, 0x7A,
|
||||
0x61, 0x48, 0x2F, 0x16, 0xE8,
|
||||
0xCF, 0xB6, 0x9D, 0x84, 0x6B,
|
||||
0x52, 0x39, 0x20, 0x07, 0xD9,
|
||||
0xC0, 0xA7, 0x8E, 0x75, 0x5C,
|
||||
0x43, 0x2A, 0x11, 0xE3, 0xCA,
|
||||
0xB1, 0x98, 0x7F, 0x66, 0x4D,
|
||||
0x34, 0x1B, 0x00, 0x1D, 0x03
|
||||
};
|
||||
|
||||
#include "iface_cc2500.h"
|
||||
uint8_t hop(uint8_t byte)
|
||||
{
|
||||
return pgm_read_byte_near(&hop_data[byte]);
|
||||
}
|
||||
|
||||
uint16_t initFrSkyX()
|
||||
{
|
||||
while(!chanskip)
|
||||
{
|
||||
randomSeed((uint32_t)analogRead(A6) << 10 | analogRead(A7));
|
||||
chanskip=random(0xfefefefe)%47;
|
||||
}
|
||||
while((chanskip-ctr)%4)
|
||||
ctr=(ctr+1)%4;
|
||||
|
||||
counter_rst=(chanskip-ctr)>>2;
|
||||
//for test***************
|
||||
//rx_tx_addr[3]=0xB3;
|
||||
//rx_tx_addr[2]=0xFD;
|
||||
//************************
|
||||
frskyX_init();
|
||||
//
|
||||
if(IS_AUTOBIND_FLAG_on)
|
||||
{
|
||||
state = FRSKY_BIND;
|
||||
initialize_data(1);
|
||||
}
|
||||
else
|
||||
{
|
||||
state = FRSKY_DATA1;
|
||||
initialize_data(0);
|
||||
}
|
||||
return 10000;
|
||||
}
|
||||
|
||||
uint16_t ReadFrSkyX()
|
||||
{
|
||||
switch(state)
|
||||
{
|
||||
default:
|
||||
set_start(47);
|
||||
CC2500_SetPower();
|
||||
cc2500_strobe(CC2500_SFRX);
|
||||
//
|
||||
frskyX_build_bind_packet();
|
||||
cc2500_strobe(CC2500_SIDLE);
|
||||
cc2500_writeFifo(packet, packet[0]+1);
|
||||
state++;
|
||||
return 9000;
|
||||
case FRSKY_BIND_DONE:
|
||||
initialize_data(0);
|
||||
channr=0;
|
||||
BIND_DONE;
|
||||
state++;
|
||||
break;
|
||||
case FRSKY_DATA1:
|
||||
LED_ON;
|
||||
CC2500_SetTxRxMode(TX_EN);
|
||||
set_start(channr);
|
||||
CC2500_SetPower();
|
||||
cc2500_strobe(CC2500_SFRX);
|
||||
channr = (channr+chanskip)%47;
|
||||
cc2500_strobe(CC2500_SIDLE);
|
||||
cc2500_writeFifo(packet, packet[0]+1);
|
||||
//
|
||||
frskyX_data_frame();
|
||||
state++;
|
||||
return 5500;
|
||||
case FRSKY_DATA2:
|
||||
CC2500_SetTxRxMode(RX_EN);
|
||||
cc2500_strobe(CC2500_SIDLE);
|
||||
state++;
|
||||
return 200;
|
||||
case FRSKY_DATA3:
|
||||
cc2500_strobe(CC2500_SRX);
|
||||
state++;
|
||||
return 3000;
|
||||
case FRSKY_DATA4:
|
||||
len = cc2500_readReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
|
||||
if (len &&(len<MAX_PKT))
|
||||
{
|
||||
cc2500_readFifo(pkt, len);
|
||||
#if defined TELEMETRY
|
||||
frsky_check_telemetry(pkt,len); //check if valid telemetry packets
|
||||
//parse telemetry packets here
|
||||
//The same telemetry function used by FrSky(D8).
|
||||
#endif
|
||||
}
|
||||
state = FRSKY_DATA1;
|
||||
return 300;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
void set_start(uint8_t ch )
|
||||
{
|
||||
cc2500_strobe(CC2500_SIDLE);
|
||||
cc2500_writeReg(CC2500_23_FSCAL3, calData[ch][0]);
|
||||
cc2500_writeReg(CC2500_24_FSCAL2, calData[ch][1]);
|
||||
cc2500_writeReg(CC2500_25_FSCAL1, calData[ch][2]);
|
||||
cc2500_writeReg(CC2500_0A_CHANNR, ch==47?0:pgm_read_word(&hop_data[ch]));
|
||||
}
|
||||
|
||||
void frskyX_init()
|
||||
{
|
||||
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, 0x1E);
|
||||
cc2500_writeReg(CC2500_07_PKTCTRL1, 0x04);
|
||||
cc2500_writeReg(CC2500_08_PKTCTRL0, 0x01);
|
||||
cc2500_writeReg(CC2500_3E_PATABLE, 0xff);
|
||||
cc2500_writeReg(CC2500_0B_FSCTRL1, 0x0A);
|
||||
cc2500_writeReg(CC2500_0C_FSCTRL0, 0x00);
|
||||
cc2500_writeReg(CC2500_0D_FREQ2, 0x5c);
|
||||
cc2500_writeReg(CC2500_0E_FREQ1, 0x76);
|
||||
cc2500_writeReg(CC2500_0F_FREQ0, 0x27);
|
||||
cc2500_writeReg(CC2500_10_MDMCFG4, 0x7B);
|
||||
cc2500_writeReg(CC2500_11_MDMCFG3, 0x61);
|
||||
cc2500_writeReg(CC2500_12_MDMCFG2, 0x13);
|
||||
cc2500_writeReg(CC2500_13_MDMCFG1, 0x23);
|
||||
cc2500_writeReg(CC2500_14_MDMCFG0, 0x7a);
|
||||
cc2500_writeReg(CC2500_15_DEVIATN, 0x51);
|
||||
cc2500_writeReg(CC2500_19_FOCCFG, 0x16);
|
||||
cc2500_writeReg(CC2500_1A_BSCFG, 0x6c);
|
||||
cc2500_writeReg(CC2500_1B_AGCCTRL2,0x43);
|
||||
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_writeReg(CC2500_07_PKTCTRL1, 0x04);
|
||||
cc2500_writeReg(CC2500_0C_FSCTRL0, option);
|
||||
cc2500_strobe(CC2500_SIDLE);
|
||||
//
|
||||
for(uint8_t c=0;c < 47;c++){//calibrate hop channels
|
||||
cc2500_strobe(CC2500_SIDLE);
|
||||
cc2500_writeReg(CC2500_0A_CHANNR,pgm_read_word(&hop_data[c]));
|
||||
cc2500_strobe(CC2500_SCAL);
|
||||
delayMicroseconds(900);//
|
||||
calData[c][0] = cc2500_readReg(CC2500_23_FSCAL3);
|
||||
calData[c][1] = cc2500_readReg(CC2500_24_FSCAL2);
|
||||
calData[c][2] = cc2500_readReg(CC2500_25_FSCAL1);
|
||||
}
|
||||
cc2500_strobe(CC2500_SIDLE);
|
||||
cc2500_writeReg(CC2500_0A_CHANNR,0x00);
|
||||
cc2500_strobe(CC2500_SCAL);
|
||||
delayMicroseconds(900);
|
||||
calData[47][0] = cc2500_readReg(CC2500_23_FSCAL3);
|
||||
calData[47][1] = cc2500_readReg(CC2500_24_FSCAL2);
|
||||
calData[47][2] = cc2500_readReg(CC2500_25_FSCAL1);
|
||||
//#######END INIT########
|
||||
}
|
||||
|
||||
void initialize_data(uint8_t adr)
|
||||
{
|
||||
cc2500_writeReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack
|
||||
cc2500_writeReg(CC2500_18_MCSM0, 0x8);
|
||||
cc2500_writeReg(CC2500_09_ADDR, adr ? 0x03 : rx_tx_addr[3]);
|
||||
cc2500_writeReg(CC2500_07_PKTCTRL1,0x05);
|
||||
}
|
||||
|
||||
void frskyX_build_bind_packet()
|
||||
{
|
||||
crc=0;
|
||||
packet[0] = 0x1D;
|
||||
packet[1] = 0x03;
|
||||
packet[2] = 0x01;
|
||||
//
|
||||
packet[3] = crc_Byte(rx_tx_addr[3]);
|
||||
packet[4] = crc_Byte(rx_tx_addr[2]);
|
||||
int idx = ((state -FRSKY_BIND) % 10) * 5;
|
||||
packet[5] = crc_Byte(idx);
|
||||
packet[6] = crc_Byte(pgm_read_word(&hop_data[idx++]));
|
||||
packet[7] = crc_Byte(pgm_read_word(&hop_data[idx++]));
|
||||
packet[8] = crc_Byte(pgm_read_word(&hop_data[idx++]));
|
||||
packet[9] = crc_Byte(pgm_read_word(&hop_data[idx++]));
|
||||
packet[10] = crc_Byte(pgm_read_word(&hop_data[idx++]));
|
||||
packet[11] = crc_Byte(0x02);
|
||||
packet[12] = crc_Byte(RX_num);
|
||||
//
|
||||
for(uint8_t i=13;i<28;i++)
|
||||
packet[i]=crc_Byte(0);
|
||||
//
|
||||
packet[28]=highByte(crc);
|
||||
packet[29]=lowByte(crc);
|
||||
//
|
||||
}
|
||||
|
||||
void frskyX_data_frame()
|
||||
{
|
||||
//0x1D 0xB3 0xFD 0x02 0x56 0x07 0x15 0x00 0x00 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x08 0x00 0x00 0x00 0x00 0x00 0x00 0x96 0x12
|
||||
//
|
||||
uint8_t lpass = pass_ ;
|
||||
uint16_t chan_0 ;
|
||||
uint16_t chan_1 ;
|
||||
uint8_t flag2 = 0;
|
||||
uint8_t startChan = 0;
|
||||
crc = 0;
|
||||
//static uint8_t p = 0;
|
||||
//
|
||||
packet[0] = 0x1D;
|
||||
packet[1] = rx_tx_addr[3];
|
||||
packet[2] = rx_tx_addr[2];
|
||||
packet[3] = crc_Byte(0x02);
|
||||
//
|
||||
packet[4] = crc_Byte((ctr<<6)+channr); //*64
|
||||
packet[5] = crc_Byte(counter_rst);
|
||||
packet[6] = crc_Byte(RX_num);
|
||||
// FLAGS 00 - standard packet
|
||||
//10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
|
||||
//20 - range check packet
|
||||
packet[7] = crc_Byte(FS_flag);
|
||||
packet[8] = crc_Byte(flag2);
|
||||
//
|
||||
if ( lpass & 1 )
|
||||
startChan += 8 ;
|
||||
|
||||
for(uint8_t i = 0; i <12 ; i+=3)
|
||||
{//12 bytes
|
||||
chan_0 = scaleForPXX(startChan);
|
||||
if(lpass & 1 )
|
||||
chan_0+=2048;
|
||||
|
||||
packet[9+i] = crc_Byte(lowByte(chan_0));//3 bytes*4
|
||||
startChan++;
|
||||
chan_1 = scaleForPXX(startChan);
|
||||
if(lpass & 1 )
|
||||
chan_1+= 2048;
|
||||
|
||||
startChan++;
|
||||
packet[9+i+1]=crc_Byte((((chan_0>>8) & 0x0F)|(chan_1 << 4)));
|
||||
packet[9+i+2]=crc_Byte(chan_1>>4);
|
||||
}
|
||||
//packet[21]=crc_Byte(0x08);//first
|
||||
packet[21]=crc_Byte(0x80);//??? when received first telemetry frame is changed to 0x80
|
||||
//packet[21]=crc_Byte(ptr[p]);//???
|
||||
//p=(p+1)%4;//repeating 4 bytes sequence pattern every 4th frame.
|
||||
|
||||
pass_=lpass+1;
|
||||
|
||||
for (uint8_t i=22;i<28;i++)
|
||||
packet[i]=crc_Byte(0);
|
||||
|
||||
packet[28]=highByte(crc);
|
||||
packet[29]=lowByte(crc);
|
||||
}
|
||||
|
||||
#endif
|
||||
uint16_t scaleForPXX( uint8_t i )
|
||||
{ //mapped 860,2140(125%) range to 64,1984(PXX values);
|
||||
return (uint16_t)(((Servo_data[i]-PPM_MIN)*3)>>1)+64;
|
||||
}
|
||||
|
||||
uint8_t crc_Byte( uint8_t byte )
|
||||
{
|
||||
crc = (crc<<8) ^ pgm_read_word(&CRCTable[((uint8_t)(crc>>8) ^ byte) & 0xFF]);
|
||||
return byte;
|
||||
}
|
||||
#endif
|
||||
Reference in New Issue
Block a user