diff --git a/Multiprotocol/AFHDS2A_a7105.ino b/Multiprotocol/AFHDS2A_a7105.ino
index 3c7b844..4333972 100644
--- a/Multiprotocol/AFHDS2A_a7105.ino
+++ b/Multiprotocol/AFHDS2A_a7105.ino
@@ -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;
diff --git a/Multiprotocol/Convert.ino b/Multiprotocol/Convert.ino
new file mode 100644
index 0000000..edfc26e
--- /dev/null
+++ b/Multiprotocol/Convert.ino
@@ -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 .
+ */
+
+/************************/
+/** 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(valmin) 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;
+}
diff --git a/Multiprotocol/DSM_cyrf6936.ino b/Multiprotocol/DSM_cyrf6936.ino
index faea637..fc18528 100644
--- a/Multiprotocol/DSM_cyrf6936.ino
+++ b/Multiprotocol/DSM_cyrf6936.ino
@@ -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
diff --git a/Multiprotocol/Common.ino b/Multiprotocol/FrSkyDVX_common.ino
similarity index 60%
rename from Multiprotocol/Common.ino
rename to Multiprotocol/FrSkyDVX_common.ino
index 14c05af..8f257ed 100644
--- a/Multiprotocol/Common.ino
+++ b/Multiprotocol/FrSkyDVX_common.ino
@@ -13,169 +13,27 @@
along with Multiprotocol. If not, see .
*/
-#ifdef ENABLE_PPM
-void InitPPM()
-{
- for(uint8_t i=0;i1844
- 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(valmin) 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
diff --git a/Multiprotocol/FrSkyD_cc2500.ino b/Multiprotocol/FrSkyD_cc2500.ino
index 9cfabf0..bd82890 100644
--- a/Multiprotocol/FrSkyD_cc2500.ino
+++ b/Multiprotocol/FrSkyD_cc2500.ino
@@ -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
}
}
diff --git a/Multiprotocol/FrSkyX_Rx_cc2500.ino b/Multiprotocol/FrSkyX_Rx_cc2500.ino
index 4b9ea1a..5bec6c6 100644
--- a/Multiprotocol/FrSkyX_Rx_cc2500.ino
+++ b/Multiprotocol/FrSkyX_Rx_cc2500.ino
@@ -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;
}
diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino
index 7fa5de1..55b7696 100644
--- a/Multiprotocol/FrSkyX_cc2500.ino
+++ b/Multiprotocol/FrSkyX_cc2500.ino
@@ -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>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
\ No newline at end of file
diff --git a/Multiprotocol/Hitec_cc2500.ino b/Multiprotocol/Hitec_cc2500.ino
index 760e61d..e31bac0 100644
--- a/Multiprotocol/Hitec_cc2500.ino
+++ b/Multiprotocol/Hitec_cc2500.ino
@@ -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 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
diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h
index 5b7c5a0..3dd633a 100644
--- a/Multiprotocol/Multiprotocol.h
+++ b/Multiprotocol/Multiprotocol.h
@@ -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
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index 8315f79..e4575c5 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -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;iCCR2=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
diff --git a/Multiprotocol/Scanner_cc2500.ino b/Multiprotocol/Scanner_cc2500.ino
index 6b09b21..3210c80 100644
--- a/Multiprotocol/Scanner_cc2500.ino
+++ b/Multiprotocol/Scanner_cc2500.ino
@@ -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) {
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index da0b08c..b338dfe 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -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;i0 && 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
- }
+ telemetry_in_buffer[i]=packet_in[i]; // Buffer telemetry values to be sent
+
+ //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]) )
- {
- // 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 ;
+ 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
- FrX_receive_seq = ( FrX_receive_seq + 1 ) & 0x03 ;
+ if(packet_in[4] & 0x80)
+ RX_RSSI=packet_in[4] & 0x7F ;
+ else
+ RxBt = (packet_in[4]<<1) + 1 ;
- if ( FrskyxRxTelemetryValidSequence & 0x80 )
- {
- FrX_receive_seq = ( FrskyxRxTelemetryValidSequence + 1 ) & 3 ;
- FrskyxRxTelemetryValidSequence &= 0x7F ;
- }
+ //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 )
+ {
+ 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 ;
-
- 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 ;
+ q->count = 0 ;
+ q->valid = true ;
+
+ 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;iCR1 &= ~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
diff --git a/Multiprotocol/WFLY_cyrf6936.ino b/Multiprotocol/WFLY_cyrf6936.ino
index baacbcd..c7664da 100644
--- a/Multiprotocol/WFLY_cyrf6936.ino
+++ b/Multiprotocol/WFLY_cyrf6936.ino
@@ -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;