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

Initial S.Port send

Browse Source 
!!! No retransmit for now !!!
This commit is contained in:
pascallanger 2019-09-30 17:35:12 +02:00
parent b6df650f50
commit 821732bba9
13 changed files with 596 additions and 507 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Multiprotocol/AFHDS2A_a7105.ino
View File

@ -90,14 +90,14 @@ static void AFHDS2A_update_telemetry()
#ifdef AFHDS2A_FW_TELEMETRY
if (option & 0x80)
{// forward 0xAA and 0xAC telemetry to TX, skip rx and tx id to save space
pkt[0]= TX_RSSI;
packet_in[0]= TX_RSSI;
debug("T(%02X)=",packet[0]);
for(uint8_t i=9;i < AFHDS2A_RXPACKET_SIZE; i++)
{
pkt[i-8]=packet[i];
packet_in[i-8]=packet[i];
debug(" %02X",packet[i]);
}
pkt[29]=packet[0]; // 0xAA Normal telemetry, 0xAC Extended telemetry
packet_in[29]=packet[0]; // 0xAA Normal telemetry, 0xAC Extended telemetry
telemetry_link=2;
debugln("");
return;

142
Multiprotocol/Convert.ino Normal file
View File

@ -0,0 +1,142 @@
/*
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/>.
*/
/************************/
/** Convert routines **/
/************************/
// Reverse a channel and store it
void reverse_channel(uint8_t num)
{
uint16_t val=2048-Channel_data[num];
if(val>=2048) val=2047;
Channel_data[num]=val;
}
// Channel value is converted to ppm 860<->2140 -125%<->+125% and 988<->2012 -100%<->+100%
uint16_t convert_channel_ppm(uint8_t num)
{
uint16_t val=Channel_data[num];
return (((val<<2)+val)>>3)+860; //value range 860<->2140 -125%<->+125%
}
// Channel value 100% is converted to 10bit values 0<->1023
uint16_t convert_channel_10b(uint8_t num)
{
uint16_t val=Channel_data[num];
val=((val<<2)+val)>>3;
if(val<=128) return 0;
if(val>=1152) return 1023;
return val-128;
}
// Channel value 100% is converted to 8bit values 0<->255
uint8_t convert_channel_8b(uint8_t num)
{
uint16_t val=Channel_data[num];
val=((val<<2)+val)>>5;
if(val<=32) return 0;
if(val>=288) return 255;
return val-32;
}
// Channel value 100% is converted to 8b with deadband
uint8_t convert_channel_8b_limit_deadband(uint8_t num,uint8_t min,uint8_t mid, uint8_t max, uint8_t deadband)
{
uint16_t val=limit_channel_100(num); // 204<->1844
uint16_t db_low=CHANNEL_MID-deadband, db_high=CHANNEL_MID+deadband; // 1024+-deadband
int32_t calc;
uint8_t out;
if(val>=db_low && val<=db_high)
return mid;
else if(val<db_low)
{
val-=CHANNEL_MIN_100;
calc=mid-min;
calc*=val;
calc/=(db_low-CHANNEL_MIN_100);
out=calc;
out+=min;
}
else
{
val-=db_high;
calc=max-mid;
calc*=val;
calc/=(CHANNEL_MAX_100-db_high+1);
out=calc;
out+=mid;
if(max>min) out++; else out--;
}
return out;
}
// Channel value 100% is converted to value scaled
int16_t convert_channel_16b_limit(uint8_t num,int16_t min,int16_t max)
{
int32_t val=limit_channel_100(num); // 204<->1844
val=(val-CHANNEL_MIN_100)*(max-min)/(CHANNEL_MAX_100-CHANNEL_MIN_100)+min;
return (uint16_t)val;
}
// Channel value -125%<->125% is scaled to 16bit value with no limit
int16_t convert_channel_16b_nolimit(uint8_t num, int16_t min, int16_t max)
{
int32_t val=Channel_data[num]; // 0<->2047
val=(val-CHANNEL_MIN_100)*(max-min)/(CHANNEL_MAX_100-CHANNEL_MIN_100)+min;
return (uint16_t)val;
}
// 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 limited to 100%
uint16_t limit_channel_100(uint8_t num)
{
if(Channel_data[num]>=CHANNEL_MAX_100)
return CHANNEL_MAX_100;
if (Channel_data[num]<=CHANNEL_MIN_100)
return CHANNEL_MIN_100;
return Channel_data[num];
}
// Channel value is converted for HK310
void convert_channel_HK310(uint8_t num, uint8_t *low, uint8_t *high)
{
uint16_t temp=0xFFFF-(3440+((Channel_data[num]*5)>>1))/3;
*low=(uint8_t)(temp&0xFF);
*high=(uint8_t)(temp>>8);
}
#ifdef FAILSAFE_ENABLE
// Failsafe value is converted for HK310
void convert_failsafe_HK310(uint8_t num, uint8_t *low, uint8_t *high)
{
uint16_t temp=0xFFFF-(3440+((Failsafe_data[num]*5)>>1))/3;
*low=(uint8_t)(temp&0xFF);
*high=(uint8_t)(temp>>8);
}
#endif
// Channel value for FrSky (PPM is multiplied by 1.5)
uint16_t convert_channel_frsky(uint8_t num)
{
uint16_t val=Channel_data[num];
return ((val*15)>>4)+1290;
}

22
Multiprotocol/DSM_cyrf6936.ino
View File

@ -363,12 +363,12 @@ static uint8_t __attribute__((unused)) DSM_Check_RX_packet()
uint16_t sum = 384 - 0x10;
for(uint8_t i = 1; i < 9; i++)
{
sum += pkt[i];
sum += packet_in[i];
if(i<5)
if(pkt[i] != (0xff ^ cyrfmfg_id[i-1]))
if(packet_in[i] != (0xff ^ cyrfmfg_id[i-1]))
result=0; // bad packet
}
if( pkt[9] != (sum>>8) && pkt[10] != (uint8_t)sum )
if( packet_in[9] != (sum>>8) && packet_in[10] != (uint8_t)sum )
result=0;
return result;
}
@ -417,12 +417,12 @@ uint16_t ReadDsm()
{ // data received with no errors
CYRF_WriteRegister(CYRF_07_RX_IRQ_STATUS, 0x80); // need to set RXOW before data read
len=CYRF_ReadRegister(CYRF_09_RX_COUNT);
if(len>MAX_PKT-2)
len=MAX_PKT-2;
CYRF_ReadDataPacketLen(pkt+1, len);
if(len>TELEMETRY_BUFFER_SIZE-2)
len=TELEMETRY_BUFFER_SIZE-2;
CYRF_ReadDataPacketLen(packet_in+1, len);
if(len==10 && DSM_Check_RX_packet())
{
pkt[0]=0x80;
packet_in[0]=0x80;
telemetry_link=1; // send received data on serial
phase++;
return 2000;
@ -502,10 +502,10 @@ uint16_t ReadDsm()
{ // good data (complete with no errors)
CYRF_WriteRegister(CYRF_07_RX_IRQ_STATUS, 0x80); // need to set RXOW before data read
len=CYRF_ReadRegister(CYRF_09_RX_COUNT);
if(len>MAX_PKT-2)
len=MAX_PKT-2;
CYRF_ReadDataPacketLen(pkt+1, len);
pkt[0]=CYRF_ReadRegister(CYRF_13_RSSI)&0x1F;// store RSSI of the received telemetry signal
if(len>TELEMETRY_BUFFER_SIZE-2)
len=TELEMETRY_BUFFER_SIZE-2;
CYRF_ReadDataPacketLen(packet_in+1, len);
packet_in[0]=CYRF_ReadRegister(CYRF_13_RSSI)&0x1F;// store RSSI of the received telemetry signal
telemetry_link=1;
}
CYRF_WriteRegister(CYRF_29_RX_ABORT, 0x20); // Abort RX operation

152
Multiprotocol/Common.ino → Multiprotocol/FrSkyDVX_common.ino
View File

@ -13,169 +13,27 @@
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
#ifdef ENABLE_PPM
void InitPPM()
{
for(uint8_t i=0;i<NUM_CHN;i++)
PPM_data[i]=PPM_MAX_100+PPM_MIN_100;
PPM_data[THROTTLE]=PPM_MIN_100*2;
}
#endif
void InitChannel()
{
for(uint8_t i=0;i<NUM_CHN;i++)
Channel_data[i]=1024;
#ifdef FAILSAFE_THROTTLE_LOW_VAL
Channel_data[THROTTLE]=(uint16_t)FAILSAFE_THROTTLE_LOW_VAL; //0=-125%, 204=-100%
#else
Channel_data[THROTTLE]=204;
#endif
}
/************************/
/** Convert routines **/
/************************/
// Convert channel 8b with limit and deadband
uint8_t convert_channel_8b_limit_deadband(uint8_t num,uint8_t min,uint8_t mid, uint8_t max, uint8_t deadband)
{
uint16_t val=limit_channel_100(num); // 204<->1844
uint16_t db_low=CHANNEL_MID-deadband, db_high=CHANNEL_MID+deadband; // 1024+-deadband
int32_t calc;
uint8_t out;
if(val>=db_low && val<=db_high)
return mid;
else if(val<db_low)
{
val-=CHANNEL_MIN_100;
calc=mid-min;
calc*=val;
calc/=(db_low-CHANNEL_MIN_100);
out=calc;
out+=min;
}
else
{
val-=db_high;
calc=max-mid;
calc*=val;
calc/=(CHANNEL_MAX_100-db_high+1);
out=calc;
out+=mid;
if(max>min) out++; else out--;
}
return out;
}
// Reverse a channel and store it
void reverse_channel(uint8_t num)
{
uint16_t val=2048-Channel_data[num];
if(val>=2048) val=2047;
Channel_data[num]=val;
}
// Channel value is converted to ppm 860<->2140 -125%<->+125% and 988<->2012 -100%<->+100%
uint16_t convert_channel_ppm(uint8_t num)
{
uint16_t val=Channel_data[num];
return (((val<<2)+val)>>3)+860; //value range 860<->2140 -125%<->+125%
}
// Channel value 100% is converted to 10bit values 0<->1023
uint16_t convert_channel_10b(uint8_t num)
{
uint16_t val=Channel_data[num];
val=((val<<2)+val)>>3;
if(val<=128) return 0;
if(val>=1152) return 1023;
return val-128;
}
// Channel value 100% is converted to 8bit values 0<->255
uint8_t convert_channel_8b(uint8_t num)
{
uint16_t val=Channel_data[num];
val=((val<<2)+val)>>5;
if(val<=32) return 0;
if(val>=288) return 255;
return val-32;
}
// Channel value 100% is converted to value scaled
int16_t convert_channel_16b_limit(uint8_t num,int16_t min,int16_t max)
{
int32_t val=limit_channel_100(num); // 204<->1844
val=(val-CHANNEL_MIN_100)*(max-min)/(CHANNEL_MAX_100-CHANNEL_MIN_100)+min;
return (uint16_t)val;
}
// Channel value -125%<->125% is scaled to 16bit value with no limit
int16_t convert_channel_16b_nolimit(uint8_t num, int16_t min, int16_t max)
{
int32_t val=Channel_data[num]; // 0<->2047
val=(val-CHANNEL_MIN_100)*(max-min)/(CHANNEL_MAX_100-CHANNEL_MIN_100)+min;
return (uint16_t)val;
}
// 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 limited to 100%
uint16_t limit_channel_100(uint8_t num)
{
if(Channel_data[num]>=CHANNEL_MAX_100)
return CHANNEL_MAX_100;
if (Channel_data[num]<=CHANNEL_MIN_100)
return CHANNEL_MIN_100;
return Channel_data[num];
}
// Channel value is converted for HK310
void convert_channel_HK310(uint8_t num, uint8_t *low, uint8_t *high)
{
uint16_t temp=0xFFFF-(3440+((Channel_data[num]*5)>>1))/3;
*low=(uint8_t)(temp&0xFF);
*high=(uint8_t)(temp>>8);
}
#ifdef FAILSAFE_ENABLE
// Failsafe value is converted for HK310
void convert_failsafe_HK310(uint8_t num, uint8_t *low, uint8_t *high)
{
uint16_t temp=0xFFFF-(3440+((Failsafe_data[num]*5)>>1))/3;
*low=(uint8_t)(temp&0xFF);
*high=(uint8_t)(temp>>8);
}
#endif
// Channel value for FrSky (PPM is multiplied by 1.5)
uint16_t convert_channel_frsky(uint8_t num)
{
uint16_t val=Channel_data[num];
return ((val*15)>>4)+1290;
}
/******************************/
/** FrSky D and X routines **/
/******************************/
#if defined(FRSKYX_CC2500_INO) || defined(FRSKYX_RX_CC2500_INO)
//**CRC**
const uint16_t PROGMEM frskyX_CRC_Short[]={
const uint16_t PROGMEM FrSkyX_CRC_Short[]={
0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7 };
static uint16_t __attribute__((unused)) frskyX_CRCTable(uint8_t val)
static uint16_t __attribute__((unused)) FrSkyX_CRCTable(uint8_t val)
{
uint16_t word ;
word = pgm_read_word(&frskyX_CRC_Short[val&0x0F]) ;
word = pgm_read_word(&FrSkyX_CRC_Short[val&0x0F]) ;
val /= 16 ;
return word ^ (0x1081 * val) ;
}
uint16_t frskyX_crc_x(uint8_t *data, uint8_t len)
uint16_t FrSkyX_crc(uint8_t *data, uint8_t len)
{
uint16_t crc = 0;
for(uint8_t i=0; i < len; i++)
crc = (crc<<8) ^ frskyX_CRCTable((uint8_t)(crc>>8) ^ *data++);
crc = (crc<<8) ^ FrSkyX_CRCTable((uint8_t)(crc>>8) ^ *data++);
return crc;
}
#endif

8
Multiprotocol/FrSkyD_cc2500.ino
View File

@ -159,12 +159,12 @@ uint16_t ReadFrSky_2way()
len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if (len && len<=(0x11+3))// 20bytes
{
CC2500_ReadData(pkt, len); //received telemetry packets
CC2500_ReadData(packet_in, len); //received telemetry packets
#if defined(TELEMETRY)
if(pkt[len-1] & 0x80)
if(packet_in[len-1] & 0x80)
{//with valid crc
packet_count=0;
frsky_check_telemetry(pkt,len); //check if valid telemetry packets and buffer them.
frsky_check_telemetry(packet_in,len); //check if valid telemetry packets and buffer them.
}
#endif
}
@ -177,7 +177,7 @@ uint16_t ReadFrSky_2way()
packet_count=0;
#if defined TELEMETRY
telemetry_link=0;//no link frames
pkt[6]=0;//no user frames.
packet_in[6]=0;//no user frames.
#endif
}
}

14
Multiprotocol/FrSkyX_Rx_cc2500.ino
View File

@ -127,7 +127,7 @@ static void __attribute__((unused)) frskyx_rx_calibrate()
static uint8_t __attribute__((unused)) frskyx_rx_check_crc()
{
uint8_t limit = packet_length - 4;
uint16_t lcrc = frskyX_crc_x(&packet[3], limit - 3); // computed crc
uint16_t lcrc = FrSkyX_crc(&packet[3], limit - 3); // computed crc
uint16_t rcrc = (packet[limit] << 8) | (packet[limit + 1] & 0xff); // received crc
return lcrc == rcrc;
}
@ -159,17 +159,17 @@ static void __attribute__((unused)) frskyx_rx_build_telemetry_packet()
}
// buid telemetry packet
pkt[idx++] = RX_LQI;
pkt[idx++] = RX_RSSI;
pkt[idx++] = 0; // start channel
pkt[idx++] = 16; // number of channels in packet
packet_in[idx++] = RX_LQI;
packet_in[idx++] = RX_RSSI;
packet_in[idx++] = 0; // start channel
packet_in[idx++] = 16; // number of channels in packet
// pack channels
for (int i = 0; i < 16; i++) {
bits |= frskyx_rx_rc_chan[i] << bitsavailable;
bitsavailable += 11;
while (bitsavailable >= 8) {
pkt[idx++] = bits & 0xff;
packet_in[idx++] = bits & 0xff;
bits >>= 8;
bitsavailable -= 8;
}
@ -351,7 +351,7 @@ uint16_t FrSkyX_Rx_callback()
// packets per second
if (millis() - pps_timer >= 1000) {
pps_timer = millis();
debugln("%ld pps", pps_counter);
debugln("%d pps", pps_counter);
RX_LQI = pps_counter;
pps_counter = 0;
}

139
Multiprotocol/FrSkyX_cc2500.ino
View File

@ -19,20 +19,20 @@
#include "iface_cc2500.h"
uint8_t FrX_chanskip;
uint8_t FrX_send_seq ;
uint8_t FrX_receive_seq ;
uint8_t FrSkyX_chanskip;
uint8_t FrSkyX_TX_Seq ;
uint8_t FrSkyX_RX_Seq ;
#define FRX_FAILSAFE_TIMEOUT 1032
#define FrSkyX_FAILSAFE_TIMEOUT 1032
static void __attribute__((unused)) frskyX_set_start(uint8_t ch )
static void __attribute__((unused)) FrSkyX_set_start(uint8_t ch )
{
CC2500_Strobe(CC2500_SIDLE);
CC2500_WriteReg(CC2500_25_FSCAL1, calData[ch]);
CC2500_WriteReg(CC2500_0A_CHANNR, hopping_frequency[ch]);
}
static void __attribute__((unused)) frskyX_init()
static void __attribute__((unused)) FrSkyX_init()
{
FRSKY_init_cc2500((sub_protocol&2)?FRSKYXEU_cc2500_conf:FRSKYX_cc2500_conf); // LBT or FCC
//
@ -47,7 +47,7 @@ static void __attribute__((unused)) frskyX_init()
//#######END INIT########
}
static void __attribute__((unused)) frskyX_initialize_data(uint8_t adr)
static void __attribute__((unused)) FrSkyX_initialize_data(uint8_t adr)
{
CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack
CC2500_WriteReg(CC2500_18_MCSM0, 0x8);
@ -55,7 +55,7 @@ static void __attribute__((unused)) frskyX_initialize_data(uint8_t adr)
CC2500_WriteReg(CC2500_07_PKTCTRL1,0x05);
}
static void __attribute__((unused)) frskyX_build_bind_packet()
static void __attribute__((unused)) FrSkyX_build_bind_packet()
{
packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC
packet[1] = 0x03;
@ -75,7 +75,7 @@ static void __attribute__((unused)) frskyX_build_bind_packet()
//
uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
memset(&packet[13], 0, limit - 13);
uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3);
uint16_t lcrc = FrSkyX_crc(&packet[3], limit-3);
//
packet[limit++] = lcrc >> 8;
packet[limit] = lcrc;
@ -84,14 +84,14 @@ static void __attribute__((unused)) frskyX_build_bind_packet()
// 0-2047, 0 = 817, 1024 = 1500, 2047 = 2182
//64=860,1024=1500,1984=2140//Taranis 125%
static uint16_t __attribute__((unused)) frskyX_scaleForPXX( uint8_t i )
static uint16_t __attribute__((unused)) FrSkyX_scaleForPXX( uint8_t i )
{ //mapped 860,2140(125%) range to 64,1984(PXX values);
uint16_t chan_val=convert_channel_frsky(i)-1226;
if(i>7) chan_val|=2048; // upper channels offset
return chan_val;
}
#ifdef FAILSAFE_ENABLE
static uint16_t __attribute__((unused)) frskyX_scaleForPXX_FS( uint8_t i )
static uint16_t __attribute__((unused)) FrSkyX_scaleForPXX_FS( uint8_t i )
{ //mapped 1,2046(125%) range to 64,1984(PXX values);
uint16_t chan_val=((Failsafe_data[i]*15)>>4)+64;
if(Failsafe_data[i]==FAILSAFE_CHANNEL_NOPULSES)
@ -103,8 +103,8 @@ static uint16_t __attribute__((unused)) frskyX_scaleForPXX_FS( uint8_t i )
}
#endif
#define FRX_FAILSAFE_TIME 1032
static void __attribute__((unused)) frskyX_data_frame()
#define FrSkyX_FAILSAFE_TIME 1032
static void __attribute__((unused)) FrSkyX_build_packet()
{
//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
//
@ -116,7 +116,7 @@ static void __attribute__((unused)) frskyX_data_frame()
#ifdef FAILSAFE_ENABLE
static uint16_t failsafe_count=0;
static uint8_t FS_flag=0,failsafe_chan=0;
if (FS_flag == 0 && failsafe_count > FRX_FAILSAFE_TIME && chan_offset == 0 && IS_FAILSAFE_VALUES_on)
if (FS_flag == 0 && failsafe_count > FrSkyX_FAILSAFE_TIME && chan_offset == 0 && IS_FAILSAFE_VALUES_on)
{
FS_flag = 0x10;
failsafe_chan = 0;
@ -137,8 +137,8 @@ static void __attribute__((unused)) frskyX_data_frame()
packet[2] = rx_tx_addr[2];
packet[3] = 0x02;
//
packet[4] = (FrX_chanskip<<6)|hopping_frequency_no;
packet[5] = FrX_chanskip>>2;
packet[4] = (FrSkyX_chanskip<<6)|hopping_frequency_no;
packet[5] = FrSkyX_chanskip>>2;
packet[6] = RX_num;
//packet[7] = FLAGS 00 - standard packet
//10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
@ -150,35 +150,37 @@ static void __attribute__((unused)) frskyX_data_frame()
#endif
packet[8] = 0;
//
uint8_t startChan = chan_offset; for(uint8_t i = 0; i <12 ; i+=3)
uint8_t startChan = chan_offset;
for(uint8_t i = 0; i <12 ; i+=3)
{//12 bytes of channel data
#ifdef FAILSAFE_ENABLE
if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) )
chan_0 = frskyX_scaleForPXX_FS(failsafe_chan);
chan_0 = FrSkyX_scaleForPXX_FS(failsafe_chan);
else
#endif
chan_0 = frskyX_scaleForPXX(startChan);
chan_0 = FrSkyX_scaleForPXX(startChan);
startChan++;
//
#ifdef FAILSAFE_ENABLE
if( (FS_flag & 0x10) && ((failsafe_chan & 0x07) == (startChan & 0x07)) )
chan_1 = frskyX_scaleForPXX_FS(failsafe_chan);
chan_1 = FrSkyX_scaleForPXX_FS(failsafe_chan);
else
#endif
chan_1 = frskyX_scaleForPXX(startChan);
chan_1 = FrSkyX_scaleForPXX(startChan);
startChan++;
//
packet[9+i] = lowByte(chan_0); //3 bytes*4
packet[9+i+1]=(((chan_0>>8) & 0x0F)|(chan_1 << 4));
packet[9+i+2]=chan_1>>4;
}
packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start
if(sub_protocol & 0x01 ) // in X8 mode send only 8ch every 9ms
chan_offset = 0 ;
else
chan_offset^=0x08;
//sequence
packet[21] = (FrSkyX_RX_Seq << 4) | FrSkyX_TX_Seq ;//=8 at startup
uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
for (uint8_t i=22;i<limit;i++)
packet[i]=0;
@ -193,21 +195,37 @@ static void __attribute__((unused)) frskyX_data_frame()
break;
}
packet[i]=SportData[sport_index];
sport_index= (sport_index+1)& (MAX_SPORT_BUFFER-1);
sport_index= (sport_index+1) & (MAX_SPORT_BUFFER-1);
idxs++;
}
packet[22]= idxs;
#ifdef DEBUG_SERIAL
for(uint8_t i=0;i<idxs;i++)
{
Serial.print(packet[23+i],HEX);
Serial.print(" ");
}
Serial.println(" ");
#endif
debug("SPort: ");
for(uint8_t i=0;i<idxs;i++)
debug("%02X ",packet[23+i]);
debugln("");
#endif // SPORT_POLLING
uint16_t lcrc = frskyX_crc_x(&packet[3], limit-3);
#ifdef SPORT_SEND
uint8_t nbr_bytes=0;
for (uint8_t i=23;i<limit;i++)
{
if(SportHead==SportTail)
break; //buffer empty
packet[i]=SportData[SportHead];
SportHead=(SportHead+1) & (MAX_SPORT_BUFFER-1);
nbr_bytes++;
}
packet[22]=nbr_bytes;
if(nbr_bytes)
{
debug("SPort_out: ");
for(uint8_t i=0;i<nbr_bytes;i++)
debug("%02X ",packet[23+i]);
debugln("");
}
#endif // SPORT_SEND
uint16_t lcrc = FrSkyX_crc(&packet[3], limit-3);
packet[limit++]=lcrc>>8;//high byte
packet[limit]=lcrc;//low byte
}
@ -217,11 +235,11 @@ uint16_t ReadFrSkyX()
switch(state)
{
default:
frskyX_set_start(47);
FrSkyX_set_start(47);
CC2500_SetPower();
CC2500_Strobe(CC2500_SFRX);
//
frskyX_build_bind_packet();
FrSkyX_build_bind_packet();
CC2500_Strobe(CC2500_SIDLE);
CC2500_WriteData(packet, packet[0]+1);
if(IS_BIND_DONE)
@ -230,7 +248,7 @@ uint16_t ReadFrSkyX()
state++;
return 9000;
case FRSKY_BIND_DONE:
frskyX_initialize_data(0);
FrSkyX_initialize_data(0);
hopping_frequency_no=0;
BIND_DONE;
state++;
@ -242,14 +260,12 @@ uint16_t ReadFrSkyX()
prev_option = option ;
}
CC2500_SetTxRxMode(TX_EN);
frskyX_set_start(hopping_frequency_no);
FrSkyX_set_start(hopping_frequency_no);
CC2500_SetPower();
CC2500_Strobe(CC2500_SFRX);
hopping_frequency_no = (hopping_frequency_no+FrX_chanskip)%47;
hopping_frequency_no = (hopping_frequency_no+FrSkyX_chanskip)%47;
CC2500_Strobe(CC2500_SIDLE);
CC2500_WriteData(packet, packet[0]+1);
//
// frskyX_data_frame();
state++;
return 5200;
case FRSKY_DATA2:
@ -266,9 +282,9 @@ uint16_t ReadFrSkyX()
if (len && (len<=(0x0E + 3))) //Telemetry frame is 17
{
packet_count=0;
CC2500_ReadData(pkt, len);
CC2500_ReadData(packet_in, len);
#if defined TELEMETRY
frsky_check_telemetry(pkt,len); //check if valid telemetry packets
frsky_check_telemetry(packet_in,len); //check if valid telemetry packets
//parse telemetry packets here
//The same telemetry function used by FrSky(D8).
#endif
@ -279,10 +295,8 @@ uint16_t ReadFrSkyX()
// restart sequence on missed packet - might need count or timeout instead of one missed
if(packet_count>100)
{//~1sec
// seq_last_sent = 0;
// seq_last_rcvd = 8;
FrX_send_seq = 0x08 ;
// FrX_receive_seq = 0 ;
FrSkyX_TX_Seq = 0x08 ;
//FrSkyX_RX_Seq = 0 ;
packet_count=0;
#if defined TELEMETRY
telemetry_lost=1;
@ -290,11 +304,9 @@ uint16_t ReadFrSkyX()
}
CC2500_Strobe(CC2500_SFRX); //flush the RXFIFO
}
frskyX_data_frame();
if ( FrX_send_seq != 0x08 )
{
FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ;
}
FrSkyX_build_packet();
if ( FrSkyX_TX_Seq != 0x08 )
FrSkyX_TX_Seq = ( FrSkyX_TX_Seq + 1 ) & 0x03 ;
state = FRSKY_DATA1;
return 500;
}
@ -306,34 +318,33 @@ uint16_t initFrSkyX()
set_rx_tx_addr(MProtocol_id_master);
Frsky_init_hop();
packet_count=0;
while(!FrX_chanskip)
FrX_chanskip=random(0xfefefefe)%47;
while(!FrSkyX_chanskip)
FrSkyX_chanskip=random(0xfefefefe)%47;
//for test***************
//rx_tx_addr[3]=0xB3;
//rx_tx_addr[2]=0xFD;
//************************
frskyX_init();
#if defined SPORT_POLLING
#ifdef INVERT_SERIAL
start_timer4() ;
#endif
FrSkyX_init();
#ifdef SPORT_POLLING
#ifdef INVERT_SERIAL
start_timer4() ;
#endif
#endif
//
if(IS_BIND_IN_PROGRESS)
{
state = FRSKY_BIND;
frskyX_initialize_data(1);
FrSkyX_initialize_data(1);
}
else
{
state = FRSKY_DATA1;
frskyX_initialize_data(0);
FrSkyX_initialize_data(0);
}
// seq_last_sent = 0;
// seq_last_rcvd = 8;
FrX_send_seq = 0x08 ;
FrX_receive_seq = 0 ;
FrSkyX_TX_Seq = 0x08 ;
FrSkyX_RX_Seq = 0 ;
return 10000;
}
#endif

38
Multiprotocol/Hitec_cc2500.ino
View File

@ -285,14 +285,14 @@ uint16_t ReadHITEC()
return HITEC_RX1_TIMING;
case HITEC_RX2:
uint8_t len=CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
if(len && len<MAX_PKT)
if(len && len<TELEMETRY_BUFFER_SIZE)
{ // Something has been received
CC2500_ReadData(pkt, len);
if( (pkt[len-1] & 0x80) && pkt[0]==len-3 && pkt[1]==rx_tx_addr[1] && pkt[2]==rx_tx_addr[2] && pkt[3]==rx_tx_addr[3])
CC2500_ReadData(packet_in, len);
if( (packet_in[len-1] & 0x80) && packet_in[0]==len-3 && packet_in[1]==rx_tx_addr[1] && packet_in[2]==rx_tx_addr[2] && packet_in[3]==rx_tx_addr[3])
{ //valid crc && length ok && tx_id ok
debug("RX:l=%d",len);
for(uint8_t i=0;i<len;i++)
debug(",%02X",pkt[i]);
debug(",%02X",packet_in[i]);
if(IS_BIND_IN_PROGRESS)
{
if(len==13) // Bind packets have a length of 13
@ -300,17 +300,17 @@ uint16_t ReadHITEC()
debug(",bind");
boolean check=true;
for(uint8_t i=5;i<=10;i++)
if(pkt[i]!=i%10) check=false;
if((pkt[4]&0xF0)==0x70 && check)
if(packet_in[i]!=i%10) check=false;
if((packet_in[4]&0xF0)==0x70 && check)
{
bind_phase=pkt[4]+1;
bind_phase=packet_in[4]+1;
if(bind_phase==0x7B)
bind_counter=164; // in dumps the RX stops to reply at 0x7B so wait a little and exit
}
}
}
else
if( len==15 && pkt[4]==0 && pkt[12]==0 )
if( len==15 && packet_in[4]==0 && packet_in[12]==0 )
{ // Valid telemetry packets
// no station:
// 0C,1C,A1,2B,00,00,00,00,00,00,00,8D,00,64,8E -> 00 8D=>RX battery voltage 0x008D/28=5.03V
@ -323,40 +323,40 @@ uint16_t ReadHITEC()
// 0C,1C,A1,2B,00,16,00,00,00,00,00,16,00,2C,8E
// 0C,1C,A1,2B,00,17,00,00,00,42,44,17,00,48,8D -> 42=>temperature3 0x42-0x28=26°C,44=>temperature4 0x44-0x28=28°C
// 0C,1C,A1,2B,00,18,00,00,00,00,00,18,00,50,92
debug(",telem,%02x",pkt[14]&0x7F);
debug(",telem,%02x",packet_in[14]&0x7F);
#if defined(HITEC_FW_TELEMETRY) || defined(HITEC_HUB_TELEMETRY)
TX_RSSI = pkt[13];
TX_RSSI = packet_in[13];
if(TX_RSSI >=128)
TX_RSSI -= 128;
else
TX_RSSI += 128;
TX_LQI = pkt[14]&0x7F;
TX_LQI = packet_in[14]&0x7F;
#endif
#if defined(HITEC_FW_TELEMETRY)
if(sub_protocol==OPT_FW)
{
// 8 bytes telemetry packets => see at the end of this file how to fully decode it
pkt[0]=TX_RSSI; // TX RSSI
pkt[1]=TX_LQI; // TX LQI
uint8_t offset=pkt[5]==0?1:0;
packet_in[0]=TX_RSSI; // TX RSSI
packet_in[1]=TX_LQI; // TX LQI
uint8_t offset=packet_in[5]==0?1:0;
for(uint8_t i=5;i < 11; i++)
pkt[i-3]=pkt[i+offset]; // frame number followed by 5 bytes of data
packet_in[i-3]=packet_in[i+offset]; // frame number followed by 5 bytes of data
telemetry_link=2; // telemetry forward available
}
#endif
#if defined(HITEC_HUB_TELEMETRY)
if(sub_protocol==OPT_HUB)
{
switch(pkt[5]) // telemetry frame number
switch(packet_in[5]) // telemetry frame number
{
case 0x00:
v_lipo1 = (pkt[10])<<5 | (pkt[11])>>3; // calculation in float is volt=(pkt[10]<<8+pkt[11])/28
v_lipo1 = (packet_in[10])<<5 | (packet_in[11])>>3; // calculation in float is volt=(packet_in[10]<<8+packet_in[11])/28
break;
case 0x11:
v_lipo1 = (pkt[9])<<5 | (pkt[10])>>3; // calculation in float is volt=(pkt[9]<<8+pkt[10])/28
v_lipo1 = (packet_in[9])<<5 | (packet_in[10])>>3; // calculation in float is volt=(packet_in[9]<<8+packet_in[10])/28
break;
case 0x18:
v_lipo2 = (pkt[6])<<5 | (pkt[7])>>3; // calculation in float is volt=(pkt[6]<<8+pkt[7])/10
v_lipo2 = (packet_in[6])<<5 | (packet_in[7])>>3; // calculation in float is volt=(packet_in[6]<<8+packet_in[7])/10
break;
}
telemetry_link=1; // telemetry hub available

6
Multiprotocol/Multiprotocol.h
View File

@ -17,9 +17,9 @@
// Version
//******************
#define VERSION_MAJOR 1
#define VERSION_MINOR 2
#define VERSION_REVISION 1
#define VERSION_PATCH_LEVEL 84
#define VERSION_MINOR 3
#define VERSION_REVISION 0
#define VERSION_PATCH_LEVEL 1
//******************
// Protocols

112
Multiprotocol/Multiprotocol.ino
View File

@ -166,14 +166,14 @@ uint8_t RX_num;
//Serial RX variables
#define BAUD 100000
#define RXBUFFER_SIZE 26
#define RXBUFFER_SIZE 34
volatile uint8_t rx_buff[RXBUFFER_SIZE];
volatile uint8_t rx_ok_buff[RXBUFFER_SIZE];
volatile uint8_t discard_frame = 0;
// Telemetry
#define MAX_PKT 30
uint8_t pkt[MAX_PKT];//telemetry receiving packets
#define TELEMETRY_BUFFER_SIZE 30
uint8_t packet_in[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets
#if defined(TELEMETRY)
#ifdef INVERT_TELEMETRY
#if not defined(ORANGE_TX) && not defined(STM32_BOARD)
@ -183,7 +183,7 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets
#define INVERT_SERIAL 1
#endif
uint8_t pass = 0;
uint8_t pktt[MAX_PKT];//telemetry receiving packets
uint8_t telemetry_in_buffer[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets
#ifdef BASH_SERIAL
// For bit-bashed serial output
#define TXBUFFER_SIZE 192
@ -217,6 +217,11 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets
uint8_t sport_idx = 0;
uint8_t sport_index = 0;
#endif
#ifdef SPORT_SEND
#define MAX_SPORT_BUFFER 64
uint8_t SportData[MAX_SPORT_BUFFER];
uint8_t SportHead=0, SportTail=0;
#endif
#endif // TELEMETRY
// Callback
@ -399,9 +404,15 @@ void setup()
#endif
// Set default channels' value
InitChannel();
for(uint8_t i=0;i<NUM_CHN;i++)
Channel_data[i]=1024;
Channel_data[THROTTLE]=0; //0=-125%, 204=-100%
#ifdef ENABLE_PPM
InitPPM();
// Set default PPMs' value
for(uint8_t i=0;i<NUM_CHN;i++)
PPM_data[i]=PPM_MAX_100+PPM_MIN_100;
PPM_data[THROTTLE]=PPM_MIN_100*2;
#endif
// Update LED
@ -1394,6 +1405,7 @@ void update_serial_data()
{
RX_DONOTUPDATE_on;
RX_FLAG_off; //data is being processed
#ifdef SAMSON // Extremely dangerous, do not enable this unless you know what you are doing...
if( rx_ok_buff[0]==0x55 && (rx_ok_buff[1]&0x1F)==PROTO_FRSKYD && rx_ok_buff[2]==0x7F && rx_ok_buff[24]==217 && rx_ok_buff[25]==202 )
{//proto==FRSKYD+sub==7+rx_num==7+CH15==73%+CH16==73%
@ -1461,10 +1473,21 @@ void update_serial_data()
FAILSAFE_VALUES_on; //failsafe data has been received
}
#endif
#ifdef SERIAL_DATA_ENABLE
bool data=false;
if(rx_ok_buff[0]&0x20)
{ // Packet has additional data
data=true;
rx_ok_buff[0]&=0xDF; //remove the data flag
}
#endif
#ifdef BONI
if(CH14_SW)
rx_ok_buff[2]=(rx_ok_buff[2]&0xF0)|((rx_ok_buff[2]+1)&0x0F); // Extremely dangerous, do not enable this!!! This is really for a special case...
#endif
if( (rx_ok_buff[0] != cur_protocol[0]) || ((rx_ok_buff[1]&0x5F) != (cur_protocol[1]&0x5F)) || ( (rx_ok_buff[2]&0x7F) != (cur_protocol[2]&0x7F) ) )
{ // New model has been selected
CHANGE_PROTOCOL_FLAG_on; //change protocol
@ -1499,7 +1522,7 @@ void update_serial_data()
for(uint8_t i=0;i<3;i++)
cur_protocol[i] = rx_ok_buff[i];
// decode channel/failsafe values
// decode channel/failsafe values
volatile uint8_t *p=rx_ok_buff+3;
uint8_t dec=-3;
for(uint8_t i=0;i<NUM_CHN;i++)
@ -1519,6 +1542,40 @@ void update_serial_data()
#endif
Channel_data[i]=temp; //value range 0..2047, 0=-125%, 2047=+125%
}
#ifdef SERIAL_DATA_ENABLE
if(data)
{ // Data available for the current protocol
#ifdef SPORT_SEND
if(protocol==PROTO_FRSKYX)
{
#define BYTE_STUFF 0x7D
#define STUFF_MASK 0x20
//TODO detect overrun of the buffer before writing...
//debug("SPort_in: ");
SportData[SportTail]=0x7E;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
SportData[SportTail]=rx_ok_buff[26]&0x1F;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
for(uint8_t i=27;i<27+7;i++)
{
if(rx_ok_buff[i]==BYTE_STUFF)
{//stuff
SportData[SportTail]=BYTE_STUFF;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
SportData[SportTail]=rx_ok_buff[i]^STUFF_MASK;
}
else
SportData[SportTail]=rx_ok_buff[i];
//debug("%02X ",SportData[SportTail]);
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
}
//debugln("");
}
#endif
}
#endif // SERIAL_DATA_ENABLE
RX_DONOTUPDATE_off;
#ifdef ORANGE_TX
cli();
@ -1876,7 +1933,7 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
ISR(USART_RX_vect)
#endif
{ // RX interrupt
static uint8_t idx=0;
static uint8_t idx=0,len=26;
#ifdef ORANGE_TX
if((USARTC0.STATUS & 0x1C)==0) // Check frame error, data overrun and parity error
#elif defined STM32_BOARD
@ -1892,22 +1949,43 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
idx=0;discard_frame=0;
RX_MISSED_BUFF_off; // If rx_buff was good it's not anymore...
rx_buff[0]=UDR0;
#ifdef FAILSAFE_ENABLE
if((rx_buff[0]&0xFC)==0x54) // If 1st byte is 0x54, 0x55, 0x56 or 0x57 it looks ok
#ifdef SERIAL_DATA_ENABLE
#ifdef FAILSAFE_ENABLE
if((rx_buff[0]&0xDC)==0x54) // If 1st byte is 0x74, 0x75, 0x76, 0x77, 0x54, 0x55, 0x56 or 0x57 it looks ok
#else
if((rx_buff[0]&0xDE)==0x54) // If 1st byte is 0x74, 0x75, 0x54 or 0x55 it looks ok
#endif
#else
if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
#ifdef FAILSAFE_ENABLE
if((rx_buff[0]&0xFC)==0x54) // If 1st byte is 0x58, 0x54, 0x55, 0x56 or 0x57 it looks ok
#else
if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x58, 0x54 or 0x55 it looks ok
#endif
#endif
{
uint16_t max_time;
#ifdef SERIAL_DATA_ENABLE
if(rx_buff[0]&0x20)
{
max_time=8500;
len=34;
}
else
#endif
{
max_time=6500;
len=26;
}
TX_RX_PAUSE_on;
tx_pause();
#if defined STM32_BOARD
TIMER2_BASE->CCR2=TIMER2_BASE->CNT+(6500L); // Full message should be received within timer of 3250us
TIMER2_BASE->CCR2=TIMER2_BASE->CNT+max_time;// Full message should be received within timer of 3250us
TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer2/Comp2 interrupt flag
TIMER2_BASE->DIER |= TIMER_DIER_CC2IE; // Enable Timer2/Comp2 interrupt
#else
OCR1B = TCNT1+(6500L) ; // Full message should be received within timer of 3250us
TIFR1 = OCF1B_bm ; // clear OCR1B match flag
SET_TIMSK1_OCIE1B ; // enable interrupt on compare B match
OCR1B = TCNT1+max_time; // Full message should be received within timer of 3250us
TIFR1 = OCF1B_bm ; // clear OCR1B match flag
SET_TIMSK1_OCIE1B ; // enable interrupt on compare B match
#endif
idx++;
}
@ -1915,11 +1993,11 @@ static uint32_t random_id(uint16_t address, uint8_t create_new)
else
{
rx_buff[idx++]=UDR0; // Store received byte
if(idx>=RXBUFFER_SIZE)
if(idx>=len)
{ // A full frame has been received
if(!IS_RX_DONOTUPDATE_on)
{ //Good frame received and main is not working on the buffer
memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer
memcpy((void*)rx_ok_buff,(const void*)rx_buff,len);// Duplicate the buffer
RX_FLAG_on; // flag for main to process servo data
}
else

4
Multiprotocol/Scanner_cc2500.ino
View File

@ -109,7 +109,7 @@ uint16_t Scanner_callback()
if (rf_ch_num >= (SCAN_MAX_RADIOCHANNEL + 1))
rf_ch_num = 0;
if (scan_tlm_index++ == 0)
pkt[0] = rf_ch_num; // start channel for telemetry packet
packet_in[0] = rf_ch_num; // start channel for telemetry packet
Scanner_scan_next();
phase = SCAN_GET_RSSI;
}
@ -118,7 +118,7 @@ uint16_t Scanner_callback()
rssi = Scanner_scan_rssi();
if(rssi >= max_rssi) {
max_rssi = rssi;
pkt[scan_tlm_index] = rssi;
packet_in[scan_tlm_index] = rssi;
}
max_count++;
if(max_count > SCAN_MAX_COUNT) {

432
Multiprotocol/Telemetry.ino
View File

@ -37,17 +37,17 @@ uint8_t RetrySequence ;
uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS];
// Store for out of sequence packet
uint8_t FrskyxRxTelemetryValidSequence ;
struct t_fx_rx_frame
uint8_t FrSkyX_RX_ValidSeq ;
struct t_FrSkyX_RX_Frame
{
uint8_t valid ;
uint8_t count ;
uint8_t payload[6] ;
boolean valid;
uint8_t count;
uint8_t payload[6];
} ;
// Store for FrskyX telemetry
struct t_fx_rx_frame FrskyxRxFrames[4] ;
uint8_t NextFxFrameToForward ;
struct t_FrSkyX_RX_Frame FrSkyX_RX_Frames[4] ;
uint8_t FrSkyX_RX_NextFrame=0;
#ifdef SPORT_POLLING
uint8_t sport_rx_index[28] ;
uint8_t ukindex ;
@ -146,18 +146,18 @@ static void multi_send_status()
#ifdef MULTI_TELEMETRY
void DSM_frame()
{
if (pkt[0] == 0x80)
if (packet_in[0] == 0x80)
{
multi_send_header(MULTI_TELEMETRY_DSMBIND, 10);
for (uint8_t i = 1; i < 11; i++) // 10 bytes of DSM bind response
Serial_write(pkt[i]);
Serial_write(packet_in[i]);
}
else
{
multi_send_header(MULTI_TELEMETRY_DSM, 17);
for (uint8_t i = 0; i < 17; i++) // RSSI value followed by 16 bytes of telemetry data
Serial_write(pkt[i]);
Serial_write(packet_in[i]);
}
}
#else
@ -165,7 +165,7 @@ static void multi_send_status()
{
Serial_write(0xAA); // Telemetry packet
for (uint8_t i = 0; i < 17; i++) // RSSI value followed by 16 bytes of telemetry data
Serial_write(pkt[i]);
Serial_write(packet_in[i]);
}
#endif
#endif
@ -178,9 +178,9 @@ static void multi_send_status()
#else
Serial_write(0xAA); // Telemetry packet
#endif
Serial_write(pkt[0]); // start channel
Serial_write(packet_in[0]); // start channel
for(uint8_t ch = 0; ch < SCAN_CHANS_PER_PACKET; ch++)
Serial_write(pkt[ch+1]); // RSSI power levels
Serial_write(packet_in[ch+1]); // RSSI power levels
}
#endif
@ -193,7 +193,7 @@ static void multi_send_status()
Serial_write(0xAA); // Telemetry packet
#endif
for (uint8_t i = 0; i < 26; i++)
Serial_write(pkt[i]); // pps, rssi, ch start, ch count, 16x ch data
Serial_write(packet_in[i]); // pps, rssi, ch start, ch count, 16x ch data
}
#endif
@ -201,12 +201,12 @@ static void multi_send_status()
void AFHDSA_short_frame()
{
#if defined MULTI_TELEMETRY
multi_send_header(pkt[29]==0xAA?MULTI_TELEMETRY_AFHDS2A:MULTI_TELEMETRY_AFHDS2A_AC, 29);
multi_send_header(packet_in[29]==0xAA?MULTI_TELEMETRY_AFHDS2A:MULTI_TELEMETRY_AFHDS2A_AC, 29);
#else
Serial_write(pkt[29]); // Telemetry packet 0xAA or 0xAC
Serial_write(packet_in[29]); // Telemetry packet 0xAA or 0xAC
#endif
for (uint8_t i = 0; i < 29; i++) // RSSI value followed by 4*7 bytes of telemetry data
Serial_write(pkt[i]);
Serial_write(packet_in[i]);
}
#endif
@ -219,7 +219,7 @@ static void multi_send_status()
Serial_write(0xAA); // Telemetry packet
#endif
for (uint8_t i = 0; i < 8; i++) // TX RSSI and TX LQI values followed by frame number and 5 bytes of telemetry data
Serial_write(pkt[i]);
Serial_write(packet_in[i]);
}
#endif
@ -247,127 +247,155 @@ void frskySendStuffed()
Serial_write(START_STOP);
}
void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
void frsky_check_telemetry(uint8_t *packet_in,uint8_t len)
{
uint8_t clen = pkt[0] + 3 ;
if(pkt[1] == rx_tx_addr[3] && pkt[2] == rx_tx_addr[2] && len == clen )
if(packet_in[1] != rx_tx_addr[3] || packet_in[2] != rx_tx_addr[2] || len != packet_in[0] + 3 )
return; // Bad address or length...
telemetry_link|=1; // Telemetry data is available
// RSSI and LQI are the 2 last bytes
TX_RSSI = packet_in[len-2];
if(TX_RSSI >=128)
TX_RSSI -= 128;
else
TX_RSSI += 128;
TX_LQI = packet_in[len-1]&0x7F;
#if defined FRSKYD_CC2500_INO
if (protocol==PROTO_FRSKYD)
{
telemetry_link|=1; // Telemetry data is available
TX_RSSI = pkt[len-2];
if(TX_RSSI >=128)
TX_RSSI -= 128;
else
TX_RSSI += 128;
TX_LQI = pkt[len-1]&0x7F;
//Save current buffer
for (uint8_t i=3;i<len-2;i++)
pktt[i]=pkt[i]; // Buffer telemetry values to be sent
telemetry_in_buffer[i]=packet_in[i]; // Buffer telemetry values to be sent
if(pktt[6]>0 && pktt[6]<=10)
{
if (protocol==PROTO_FRSKYD)
{
if ( ( pktt[7] & 0x1F ) == (telemetry_counter & 0x1F) )
{
uint8_t topBit = 0 ;
if ( telemetry_counter & 0x80 )
if ( ( telemetry_counter & 0x1F ) != RetrySequence )
topBit = 0x80 ;
telemetry_counter = ( (telemetry_counter+1)%32 ) | topBit ; // Request next telemetry frame
}
else
{
// incorrect sequence
RetrySequence = pktt[7] & 0x1F ;
telemetry_counter |= 0x80 ;
pktt[6]=0 ; // Discard current packet and wait for retransmit
}
//Check incoming telemetry sequence
if(telemetry_in_buffer[6]>0 && telemetry_in_buffer[6]<=10)
{ //Telemetry length ok
if ( ( telemetry_in_buffer[7] & 0x1F ) == (telemetry_counter & 0x1F) )
{//Sequence is ok
uint8_t topBit = 0 ;
if ( telemetry_counter & 0x80 )
if ( ( telemetry_counter & 0x1F ) != RetrySequence )
topBit = 0x80 ;
telemetry_counter = ( (telemetry_counter+1)%32 ) | topBit ; // Request next telemetry frame
}
else
{//Incorrect sequence
RetrySequence = telemetry_in_buffer[7] & 0x1F ;
telemetry_counter |= 0x80 ;
telemetry_in_buffer[6]=0 ; // Discard current packet and wait for retransmit
}
}
else
pktt[6]=0; // Discard packet
//
telemetry_in_buffer[6]=0; // Discard packet
}
#endif
#if defined SPORT_TELEMETRY && defined FRSKYX_CC2500_INO
if (protocol==PROTO_FRSKYX)
{
/*Telemetry frames(RF) SPORT info
15 bytes payload
SPORT frame valid 6+3 bytes
[00] PKLEN 0E 0E 0E 0E
[01] TXID1 DD DD DD DD
[02] TXID2 6D 6D 6D 6D
[03] CONST 02 02 02 02
[04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
[05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
[06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
[07] STRM1 00 00 7E 00
[08] STRM2 00 00 1A 00
[09] STRM3 00 00 10 00
[10] STRM4 03 03 03 03
[11] STRM5 F1 F1 F1 F1
[12] STRM6 D1 D1 D0 D0
[13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
[14] CHKSUM2 --|*/
telemetry_lost=0;
if (protocol==PROTO_FRSKYX)
{
uint16_t lcrc = frskyX_crc_x(&pkt[3], len-7 ) ;
if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) )
uint16_t lcrc = FrSkyX_crc(&packet_in[3], len-7 ) ;
if ( ( (lcrc >> 8) != packet_in[len-4]) || ( (lcrc & 0x00FF ) != packet_in[len-3]) )
return; // Bad CRC
if(packet_in[4] & 0x80)
RX_RSSI=packet_in[4] & 0x7F ;
else
RxBt = (packet_in[4]<<1) + 1 ;
//Check incoming telemetry sequence
uint8_t packet_seq=packet_in[5] & 0x03;
if ( (packet_in[5] & 0x0F) == 0x08 )
{//Request init
FrSkyX_RX_Seq = 0x08 ;
FrSkyX_RX_NextFrame = 0x00 ;
FrSkyX_RX_Frames[0].valid = false ;
FrSkyX_RX_Frames[1].valid = false ;
FrSkyX_RX_Frames[2].valid = false ;
FrSkyX_RX_Frames[3].valid = false ;
}
else if ( packet_seq == (FrSkyX_RX_Seq & 0x03 ) )
{//In sequence
struct t_FrSkyX_RX_Frame *p ;
uint8_t count ;
// packet_in[4] RSSI
// packet_in[5] sequence control
// packet_in[6] payload count
// packet_in[7-12] payload
p = &FrSkyX_RX_Frames[packet_seq] ;
count = packet_in[6]; // Payload length
if ( count <= 6 )
{//Store payload
p->count = count ;
for ( uint8_t i = 0 ; i < count ; i++ )
p->payload[i] = packet_in[i+7] ;
}
else
p->count = 0 ; // Discard
p->valid = true ;
FrSkyX_RX_Seq = ( FrSkyX_RX_Seq + 1 ) & 0x03 ; // Move to next sequence
if ( FrSkyX_RX_ValidSeq & 0x80 )
{
// Check if in sequence
if ( (pkt[5] & 0x0F) == 0x08 )
{
FrX_receive_seq = 0x08 ;
NextFxFrameToForward = 0 ;
FrskyxRxFrames[0].valid = 0 ;
FrskyxRxFrames[1].valid = 0 ;
FrskyxRxFrames[2].valid = 0 ;
FrskyxRxFrames[3].valid = 0 ;
}
else if ( (pkt[5] & 0x03) == (FrX_receive_seq & 0x03 ) )
{
// OK to process
struct t_fx_rx_frame *p ;
uint8_t count ;
p = &FrskyxRxFrames[FrX_receive_seq & 3] ;
count = pkt[6] ;
if ( count <= 6 )
{
p->count = count ;
for ( uint8_t i = 0 ; i < count ; i += 1 )
p->payload[i] = pkt[i+7] ;
}
else
p->count = 0 ;
p->valid = 1 ;
FrX_receive_seq = ( FrX_receive_seq + 1 ) & 0x03 ;
if ( FrskyxRxTelemetryValidSequence & 0x80 )
{
FrX_receive_seq = ( FrskyxRxTelemetryValidSequence + 1 ) & 3 ;
FrskyxRxTelemetryValidSequence &= 0x7F ;
}
FrSkyX_RX_Seq = ( FrSkyX_RX_ValidSeq + 1 ) & 3 ;
FrSkyX_RX_ValidSeq &= 0x7F ;
}
}
else
{//Not in sequence
struct t_FrSkyX_RX_Frame *q ;
uint8_t count ;
// packet_in[4] RSSI
// packet_in[5] sequence control
// packet_in[6] payload count
// packet_in[7-12] payload
if ( packet_seq == ( ( FrSkyX_RX_Seq +1 ) & 3 ) )
{//Received next sequence -> save it
q = &FrSkyX_RX_Frames[packet_seq] ;
count = packet_in[6]; // Payload length
if ( count <= 6 )
{//Store payload
q->count = count ;
for ( uint8_t i = 0 ; i < count ; i++ )
q->payload[i] = packet_in[i+7] ;
}
else
{
// Save and request correct packet
struct t_fx_rx_frame *q ;
uint8_t count ;
// pkt[4] RSSI
// pkt[5] sequence control
// pkt[6] payload count
// pkt[7-12] payload
pktt[6] = 0 ; // Don't process
if ( (pkt[5] & 0x03) == ( ( FrX_receive_seq +1 ) & 3 ) )
{
q = &FrskyxRxFrames[(pkt[5] & 0x03)] ;
count = pkt[6] ;
if ( count <= 6 )
{
q->count = count ;
for ( uint8_t i = 0 ; i < count ; i += 1 )
{
q->payload[i] = pkt[i+7] ;
}
}
else
q->count = 0 ;
q->valid = 1 ;
q->count = 0 ;
q->valid = true ;
FrskyxRxTelemetryValidSequence = 0x80 | ( pkt[5] & 0x03 ) ;
}
FrX_receive_seq = ( FrX_receive_seq & 0x03 ) | 0x04 ; // Request re-transmission
}
if (((pktt[5] >> 4) & 0x0f) == 0x08)
FrX_send_seq = 0 ;
FrSkyX_RX_ValidSeq = 0x80 | packet_seq ;
}
FrSkyX_RX_Seq = ( FrSkyX_RX_Seq & 0x03 ) | 0x04 ; // Request re-transmission of original sequence
}
#endif
//Check outgoing telemetry sequence
if (((packet_in[5] >> 4) & 0x08) == 0x08)
FrSkyX_TX_Seq = 0 ; //Request init
//debugln("s:%02X,p:%02X",FrSkyX_TX_Seq,packet_in[5] >> 4);
}
#endif
}
void init_frskyd_link_telemetry()
@ -387,9 +415,9 @@ void frsky_link_frame()
frame[0] = 0xFE; // Link frame
if (protocol==PROTO_FRSKYD)
{
frame[1] = pktt[3]; // A1
frame[2] = pktt[4]; // A2
frame[3] = pktt[5]; // RX_RSSI
frame[1] = telemetry_in_buffer[3]; // A1
frame[2] = telemetry_in_buffer[4]; // A2
frame[3] = telemetry_in_buffer[5]; // RX_RSSI
telemetry_link &= ~1 ; // Sent
telemetry_link |= 2 ; // Send hub if available
}
@ -415,26 +443,26 @@ void frsky_link_frame()
#if defined HUB_TELEMETRY
void frsky_user_frame()
{
if(pktt[6])
if(telemetry_in_buffer[6])
{//only send valid hub frames
frame[0] = 0xFD; // user frame
if(pktt[6]>USER_MAX_BYTES)
if(telemetry_in_buffer[6]>USER_MAX_BYTES)
{
frame[1]=USER_MAX_BYTES; // packet size
pktt[6]-=USER_MAX_BYTES;
telemetry_in_buffer[6]-=USER_MAX_BYTES;
telemetry_link |= 2 ; // 2 packets need to be sent
}
else
{
frame[1]=pktt[6]; // packet size
frame[1]=telemetry_in_buffer[6]; // packet size
telemetry_link=0; // only 1 packet or processing second packet
}
frame[2] = pktt[7];
frame[2] = telemetry_in_buffer[7];
for(uint8_t i=0;i<USER_MAX_BYTES;i++)
frame[i+3]=pktt[i+8];
frame[i+3]=telemetry_in_buffer[i+8];
if(telemetry_link & 2) // prepare the content of second packet
for(uint8_t i=8;i<USER_MAX_BYTES+8;i++)
pktt[i]=pktt[i+USER_MAX_BYTES];
telemetry_in_buffer[i]=telemetry_in_buffer[i+USER_MAX_BYTES];
#if defined MULTI_TELEMETRY
multi_send_frskyhub();
#else
@ -446,7 +474,7 @@ void frsky_user_frame()
}
/*
HuB RX packets.
pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
packet_in[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
%32
01 08 5E 28 12 00 5E 5E 3A 06 00 5E
0A 09 28 12 00 5E 5E 3A 06 00 5E 5E
@ -463,39 +491,39 @@ pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
#if defined SPORT_TELEMETRY
/* SPORT details serial
100K 8E2 normal-multiprotocol
-every 12ms-or multiple of 12; %36
1 2 3 4 5 6 7 8 9 CRC DESCR
7E 98 10 05 F1 20 23 0F 00 A6 SWR_ID
7E 98 10 01 F1 33 00 00 00 C9 RSSI_ID
7E 98 10 04 F1 58 00 00 00 A1 BATT_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
100K 8E2 normal-multiprotocol
-every 12ms-or multiple of 12; %36
1 2 3 4 5 6 7 8 9 CRC DESCR
7E 98 10 05 F1 20 23 0F 00 A6 SWR_ID
7E 98 10 01 F1 33 00 00 00 C9 RSSI_ID
7E 98 10 04 F1 58 00 00 00 A1 BATT_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
7E BA 10 03 F1 E2 00 00 00 18 ADC2_ID
Telemetry frames(RF) SPORT info
15 bytes payload
SPORT frame valid 6+3 bytes
[00] PKLEN 0E 0E 0E 0E
[01] TXID1 DD DD DD DD
[02] TXID2 6D 6D 6D 6D
[03] CONST 02 02 02 02
[04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
[05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
[06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
[07] STRM1 00 00 7E 00
[08] STRM2 00 00 1A 00
[09] STRM3 00 00 10 00
[10] STRM4 03 03 03 03
[11] STRM5 F1 F1 F1 F1
[12] STRM6 D1 D1 D0 D0
[13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
[14] CHKSUM2 --|
+2 appended bytes automatically RSSI and LQI/CRC bytes(len=0x0E+3);
Telemetry frames(RF) SPORT info
15 bytes payload
SPORT frame valid 6+3 bytes
[00] PKLEN 0E 0E 0E 0E
[01] TXID1 DD DD DD DD
[02] TXID2 6D 6D 6D 6D
[03] CONST 02 02 02 02
[04] RS/RB 2C D0 2C CE //D0;CE=2*RSSI;....2C = RX battery voltage(5V from Bec)
[05] HD-SK 03 10 21 32 //TX/RX telemetry hand-shake bytes
[06] NO.BT 00 00 06 03 //No.of valid SPORT frame bytes in the frame
[07] STRM1 00 00 7E 00
[08] STRM2 00 00 1A 00
[09] STRM3 00 00 10 00
[10] STRM4 03 03 03 03
[11] STRM5 F1 F1 F1 F1
[12] STRM6 D1 D1 D0 D0
[13] CHKSUM1 --|2 CRC bytes sent by RX (calculated on RX side crc16/table)
[14] CHKSUM2 --|
+2 appended bytes automatically RSSI and LQI/CRC bytes(len=0x0E+3);
0x06 0x06 0x06 0x06 0x06
@ -508,8 +536,8 @@ pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09
0xE1 0x1C 0xD0 0xEE 0x33
0x34 0x0A 0xC3 0x56 0xF3
*/
*/
#if defined SPORT_POLLING || defined MULTI_TELEMETRY
const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB,
@ -521,44 +549,30 @@ const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
#ifdef MULTI_TELEMETRY
void sportSend(uint8_t *p)
{
#ifdef SPORT_POLLING
#ifdef INVERT_SERIAL
USART3_BASE->CR1 &= ~USART_CR1_TE ;
TX_INV_on; //activate inverter for both serial TX and RX signals
USART3_BASE->CR1 |= USART_CR1_TE ;
#endif
#endif
multi_send_header(MULTI_TELEMETRY_SPORT, 9);
uint16_t crc_s = 0;
uint8_t x = p[0] ;
if ( x <= 0x1B )
x = pgm_read_byte_near( &Indices[x] ) ;
Serial_write(x) ;
for (uint8_t i = 1; i < 9; i++)
for (uint8_t i = 1; i < 8; i++)
{
if (i == 8)
p[i] = 0xff - crc_s;
Serial_write(p[i]);
if (i>0)
{
crc_s += p[i]; //0-1FF
crc_s += crc_s >> 8; //0-100
crc_s &= 0x00ff;
}
Serial_write(p[i]);
crc_s += p[i]; //0-1FF
crc_s += crc_s >> 8; //0-100
crc_s &= 0x00ff;
}
Serial_write(0xff - crc_s);
}
#else
void sportSend(uint8_t *p)
{
uint16_t crc_s = 0;
#ifdef SPORT_POLLING
#ifdef INVERT_SERIAL
USART3_BASE->CR1 &= ~USART_CR1_TE ;
TX_INV_on; //activate inverter for both serial TX and RX signals
USART3_BASE->CR1 |= USART_CR1_TE ;
#endif
#endif
#if defined SPORT_POLLING && defined INVERT_SERIAL
USART3_BASE->CR1 &= ~USART_CR1_TE ;
TX_INV_on; //activate inverter for both serial TX and RX signals
USART3_BASE->CR1 |= USART_CR1_TE ;
#endif
Serial_write(START_STOP);//+9
Serial_write(p[0]) ;
for (uint8_t i = 1; i < 9; i++)
@ -574,15 +588,11 @@ const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45,
else
Serial_write(p[i]);
if (i>0)
{
crc_s += p[i]; //0-1FF
crc_s += crc_s >> 8; //0-100
crc_s &= 0x00ff;
}
crc_s += p[i]; //0-1FF
crc_s += crc_s >> 8; //0-100
crc_s &= 0x00ff;
}
}
#endif
#if defined SPORT_POLLING
@ -668,7 +678,6 @@ void __irq_timer4(void)
TIMER4_BASE->CR1 = 0 ;
TX_INV_on; //activate inverter for both serial TX and RX signals
}
#endif
void pollSport()
@ -896,7 +905,7 @@ void proces_sport_data(uint8_t data)
{
uint8_t dest = sport * FRSKY_SPORT_PACKET_SIZE ;
uint8_t i ;
for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i += 1 )
for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i++ )
pktx1[dest++] = pktx[i] ; // Triple buffer
sport += 1 ;//ok to send
}
@ -956,33 +965,24 @@ void TelemetryUpdate()
if (protocol==PROTO_FRSKYX)
{ // FrSkyX
for(;;)
{
struct t_fx_rx_frame *p ;
{ //Empty buffer
struct t_FrSkyX_RX_Frame *p ;
uint8_t count ;
p = &FrskyxRxFrames[NextFxFrameToForward] ;
p = &FrSkyX_RX_Frames[FrSkyX_RX_NextFrame] ;
if ( p->valid )
{
count = p->count ;
for (uint8_t i=0; i < count ; i++)
proces_sport_data(p->payload[i]) ;
p->valid = 0 ; // Sent on
NextFxFrameToForward = ( NextFxFrameToForward + 1 ) & 3 ;
p->valid = false ; // Sent
FrSkyX_RX_NextFrame = ( FrSkyX_RX_NextFrame + 1 ) & 3 ;
}
else
{
break ;
}
}
if(telemetry_link)
{
if(pktt[4] & 0x80)
RX_RSSI=pktt[4] & 0x7F ;
else
RxBt = (pktt[4]<<1) + 1 ;
telemetry_link=0;
}
uint32_t now = micros();
telemetry_link=0;
sportSendFrame();
/* uint32_t now = micros();
if ((now - last) > SPORT_TIME)
{
#if defined SPORT_POLLING
@ -994,7 +994,7 @@ void TelemetryUpdate()
#else
last += SPORT_TIME ;
#endif
}
}*/
}
#endif // SPORT_TELEMETRY

16
Multiprotocol/WFLY_cyrf6936.ino
View File

@ -194,25 +194,25 @@ uint16_t ReadWFLY()
debugln("L=%02X",len)
if(len==0x10)
{
CYRF_ReadDataPacketLen(pkt, len);
CYRF_ReadDataPacketLen(packet_in, len);
debug("RX=");
for(uint8_t i=0;i<0x0F;i++)
{
debug(" %02X",pkt[i]);
if(pkt[i]==packet[i])
debug(" %02X",packet_in[i]);
if(packet_in[i]==packet[i])
check++; // Verify quickly the content
sum+=pkt[i];
sum+=packet_in[i];
}
debugln(" %02X",pkt[15]);
if(sum==pkt[15] && check>=10)
debugln(" %02X",packet_in[15]);
if(sum==packet_in[15] && check>=10)
{ // Good packet received
if(pkt[2]==0x64)
if(packet_in[2]==0x64)
{ // Switch to normal mode
BIND_DONE;
phase=WFLY_PREP_DATA;
return 10000;
}
memcpy((void *)packet,(void *)pkt,0x10); // Send back to the RX what we've just received with no modifications
memcpy((void *)packet,(void *)packet_in,0x10); // Send back to the RX what we've just received with no modifications
}
phase=WFLY_BIND_TX;
return 200;