diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino
index 52b3b46..f4fe2ba 100644
--- a/Multiprotocol/FrSkyX_cc2500.ino
+++ b/Multiprotocol/FrSkyX_cc2500.ino
@@ -1,292 +1,292 @@
-
-/* **************************
- * By Midelic on RCGroups *
- **************************
- This project is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
-
- Multiprotocol is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
-
- You should have received a copy of the GNU General Public License
- along with Multiprotocol. If not, see .
-*/
-
-#if defined(FRSKYX_CC2500_INO)
-
-#include "iface_cc2500.h"
-
-uint8_t chanskip;
-//uint8_t seq_last_sent;
-//uint8_t seq_last_rcvd;
-
-uint8_t FrX_send_seq ;
-uint8_t FrX_receive_seq ;
-
-static void __attribute__((unused)) 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()
-{
- FRSKY_init_cc2500((sub_protocol&2)?FRSKYXEU_cc2500_conf:FRSKYX_cc2500_conf); // LBT or FCC
- //
- for(uint8_t c=0;c < 48;c++)
- {//calibrate hop channels
- CC2500_Strobe(CC2500_SIDLE);
- CC2500_WriteReg(CC2500_0A_CHANNR,hopping_frequency[c]);
- CC2500_Strobe(CC2500_SCAL);
- delayMicroseconds(900);//
- calData[c] = CC2500_ReadReg(CC2500_25_FSCAL1);
- }
- //#######END INIT########
-}
-
-static void __attribute__((unused)) initialize_data(uint8_t adr)
-{
- CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack
- CC2500_WriteReg(CC2500_18_MCSM0, 0x8);
- CC2500_WriteReg(CC2500_09_ADDR, adr ? 0x03 : rx_tx_addr[3]);
- CC2500_WriteReg(CC2500_07_PKTCTRL1,0x05);
-}
-
-//**CRC**
-const uint16_t PROGMEM CRC_Short[]={
- 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
- 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7 };
-static uint16_t CRCTable(uint8_t val)
-{
- uint16_t word ;
- word = pgm_read_word(&CRC_Short[val&0x0F]) ;
- val /= 16 ;
- return word ^ (0x1081 * val) ;
-}
-static uint16_t __attribute__((unused)) crc_x(uint8_t *data, uint8_t len)
-{
- uint16_t crc = 0;
- for(uint8_t i=0; i < len; i++)
- crc = (crc<<8) ^ CRCTable((uint8_t)(crc>>8) ^ *data++);
- return crc;
-}
-
- // 0-2047, 0 = 817, 1024 = 1500, 2047 = 2182
- //64=860,1024=1500,1984=2140//Taranis 125%
-
-static uint16_t __attribute__((unused)) scaleForPXX( uint8_t i )
-{ //mapped 860,2140(125%) range to 64,1984(PXX values);
- return (uint16_t)(((Servo_data[i]-servo_min_125)*3)>>1)+64;
-}
-
-static void __attribute__((unused)) frskyX_build_bind_packet()
-{
- packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC
- packet[1] = 0x03;
- packet[2] = 0x01;
- //
- packet[3] = rx_tx_addr[3];
- packet[4] = rx_tx_addr[2];
- int idx = ((state -FRSKY_BIND) % 10) * 5;
- packet[5] = idx;
- packet[6] = hopping_frequency[idx++];
- packet[7] = hopping_frequency[idx++];
- packet[8] = hopping_frequency[idx++];
- packet[9] = hopping_frequency[idx++];
- packet[10] = hopping_frequency[idx++];
- packet[11] = 0x02;
- packet[12] = RX_num;
- //
- uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
- memset(&packet[13], 0, limit - 13);
- uint16_t lcrc = crc_x(&packet[3], limit-3);
- //
- packet[limit++] = lcrc >> 8;
- packet[limit] = lcrc;
- //
-}
-
-static void __attribute__((unused)) frskyX_data_frame()
-{
- //0x1D 0xB3 0xFD 0x02 0x56 0x07 0x15 0x00 0x00 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x08 0x00 0x00 0x00 0x00 0x00 0x00 0x96 0x12
- //
- static uint8_t lpass;
- uint16_t chan_0 ;
- uint16_t chan_1 ;
- uint8_t startChan = 0;
- //
- packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC
- packet[1] = rx_tx_addr[3];
- packet[2] = rx_tx_addr[2];
- packet[3] = 0x02;
- //
- packet[4] = (chanskip<<6)|hopping_frequency_no;
- packet[5] = chanskip>>2;
- packet[6] = RX_num;
- //packet[7] = FLAGS 00 - standard packet
- //10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
- //20 - range check packet
- packet[7] = 0;
- packet[8] = 0;
- //
- if ( lpass & 1 )
- startChan += 8 ;
-
- for(uint8_t i = 0; i <12 ; i+=3)
- {//12 bytes
- chan_0 = scaleForPXX(startChan);
- if(lpass & 1 )
- chan_0+=2048;
- startChan+=1;
- //
- chan_1 = scaleForPXX(startChan);
- if(lpass & 1 )
- chan_1+= 2048;
- startChan+=1;
- //
- 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 & 1 )// in X8 mode send only 8ch every 9ms
- lpass = 0 ;
- else
- lpass += 1 ;
-
- uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
- for (uint8_t i=22;i>8;//high byte
- packet[limit]=lcrc;//low byte
-}
-
-uint16_t ReadFrSkyX()
-{
- switch(state)
- {
- default:
- set_start(47);
- CC2500_SetPower();
- CC2500_Strobe(CC2500_SFRX);
- //
- frskyX_build_bind_packet();
- CC2500_Strobe(CC2500_SIDLE);
- CC2500_WriteData(packet, packet[0]+1);
- if(IS_BIND_DONE_on)
- state = FRSKY_BIND_DONE;
- else
- state++;
- return 9000;
- case FRSKY_BIND_DONE:
- initialize_data(0);
- hopping_frequency_no=0;
- BIND_DONE;
- state++;
- break;
- case FRSKY_DATA1:
- if ( prev_option != option )
- {
- CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack
- prev_option = option ;
- }
- CC2500_SetTxRxMode(TX_EN);
- set_start(hopping_frequency_no);
- CC2500_SetPower();
- CC2500_Strobe(CC2500_SFRX);
- hopping_frequency_no = (hopping_frequency_no+chanskip)%47;
- CC2500_Strobe(CC2500_SIDLE);
- CC2500_WriteData(packet, packet[0]+1);
- //
-// frskyX_data_frame();
- state++;
- return 5500;
- case FRSKY_DATA2:
- CC2500_SetTxRxMode(RX_EN);
- CC2500_Strobe(CC2500_SIDLE);
- state++;
- return 200;
- case FRSKY_DATA3:
- CC2500_Strobe(CC2500_SRX);
- state++;
- return 2800;
- case FRSKY_DATA4:
- len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
- if (len && (len<=(0x0E + 3))) //Telemetry frame is 17
- {
- packet_count=0;
- CC2500_ReadData(pkt, len);
- #if defined TELEMETRY
- frsky_check_telemetry(pkt,len); //check if valid telemetry packets
- //parse telemetry packets here
- //The same telemetry function used by FrSky(D8).
- #endif
- }
- else
- {
- packet_count++;
- // 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 ;
- packet_count=0;
- #if defined TELEMETRY
- telemetry_lost=1;
- #endif
- }
- CC2500_Strobe(CC2500_SFRX); //flush the RXFIFO
- }
- frskyX_data_frame();
- if ( FrX_send_seq != 0x08 )
- {
- FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ;
- }
- state = FRSKY_DATA1;
- return 500;
- }
- return 1;
-}
-
-uint16_t initFrSkyX()
-{
- set_rx_tx_addr(MProtocol_id_master);
- Frsky_init_hop();
- packet_count=0;
- while(!chanskip)
- chanskip=random(0xfefefefe)%47;
-
- //for test***************
- //rx_tx_addr[3]=0xB3;
- //rx_tx_addr[2]=0xFD;
- //************************
- frskyX_init();
- //
- if(IS_AUTOBIND_FLAG_on)
- {
- state = FRSKY_BIND;
- initialize_data(1);
- }
- else
- {
- state = FRSKY_DATA1;
- initialize_data(0);
- }
-// seq_last_sent = 0;
-// seq_last_rcvd = 8;
- uint8_t FrX_send_seq = 0x08 ;
- uint8_t FrX_receive_seq = 0 ;
- return 10000;
-}
+
+/* **************************
+ * By Midelic on RCGroups *
+ **************************
+ This project is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ Multiprotocol is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with Multiprotocol. If not, see .
+*/
+
+#if defined(FRSKYX_CC2500_INO)
+
+#include "iface_cc2500.h"
+
+uint8_t chanskip;
+//uint8_t seq_last_sent;
+//uint8_t seq_last_rcvd;
+
+uint8_t FrX_send_seq ;
+uint8_t FrX_receive_seq ;
+
+static void __attribute__((unused)) 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()
+{
+ FRSKY_init_cc2500((sub_protocol&2)?FRSKYXEU_cc2500_conf:FRSKYX_cc2500_conf); // LBT or FCC
+ //
+ for(uint8_t c=0;c < 48;c++)
+ {//calibrate hop channels
+ CC2500_Strobe(CC2500_SIDLE);
+ CC2500_WriteReg(CC2500_0A_CHANNR,hopping_frequency[c]);
+ CC2500_Strobe(CC2500_SCAL);
+ delayMicroseconds(900);//
+ calData[c] = CC2500_ReadReg(CC2500_25_FSCAL1);
+ }
+ //#######END INIT########
+}
+
+static void __attribute__((unused)) initialize_data(uint8_t adr)
+{
+ CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack
+ CC2500_WriteReg(CC2500_18_MCSM0, 0x8);
+ CC2500_WriteReg(CC2500_09_ADDR, adr ? 0x03 : rx_tx_addr[3]);
+ CC2500_WriteReg(CC2500_07_PKTCTRL1,0x05);
+}
+
+//**CRC**
+const uint16_t PROGMEM CRC_Short[]={
+ 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
+ 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7 };
+static uint16_t CRCTable(uint8_t val)
+{
+ uint16_t word ;
+ word = pgm_read_word(&CRC_Short[val&0x0F]) ;
+ val /= 16 ;
+ return word ^ (0x1081 * val) ;
+}
+static uint16_t __attribute__((unused)) crc_x(uint8_t *data, uint8_t len)
+{
+ uint16_t crc = 0;
+ for(uint8_t i=0; i < len; i++)
+ crc = (crc<<8) ^ CRCTable((uint8_t)(crc>>8) ^ *data++);
+ return crc;
+}
+
+ // 0-2047, 0 = 817, 1024 = 1500, 2047 = 2182
+ //64=860,1024=1500,1984=2140//Taranis 125%
+
+static uint16_t __attribute__((unused)) scaleForPXX( uint8_t i )
+{ //mapped 860,2140(125%) range to 64,1984(PXX values);
+ return (uint16_t)(((Servo_data[i]-servo_min_125)*3)>>1)+64;
+}
+
+static void __attribute__((unused)) frskyX_build_bind_packet()
+{
+ packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC
+ packet[1] = 0x03;
+ packet[2] = 0x01;
+ //
+ packet[3] = rx_tx_addr[3];
+ packet[4] = rx_tx_addr[2];
+ int idx = ((state -FRSKY_BIND) % 10) * 5;
+ packet[5] = idx;
+ packet[6] = hopping_frequency[idx++];
+ packet[7] = hopping_frequency[idx++];
+ packet[8] = hopping_frequency[idx++];
+ packet[9] = hopping_frequency[idx++];
+ packet[10] = hopping_frequency[idx++];
+ packet[11] = 0x02;
+ packet[12] = RX_num;
+ //
+ uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
+ memset(&packet[13], 0, limit - 13);
+ uint16_t lcrc = crc_x(&packet[3], limit-3);
+ //
+ packet[limit++] = lcrc >> 8;
+ packet[limit] = lcrc;
+ //
+}
+
+static void __attribute__((unused)) frskyX_data_frame()
+{
+ //0x1D 0xB3 0xFD 0x02 0x56 0x07 0x15 0x00 0x00 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x00 0x04 0x40 0x08 0x00 0x00 0x00 0x00 0x00 0x00 0x96 0x12
+ //
+ static uint8_t lpass;
+ uint16_t chan_0 ;
+ uint16_t chan_1 ;
+ uint8_t startChan = 0;
+ //
+ packet[0] = (sub_protocol & 2 ) ? 0x20 : 0x1D ; // LBT or FCC
+ packet[1] = rx_tx_addr[3];
+ packet[2] = rx_tx_addr[2];
+ packet[3] = 0x02;
+ //
+ packet[4] = (chanskip<<6)|hopping_frequency_no;
+ packet[5] = chanskip>>2;
+ packet[6] = RX_num;
+ //packet[7] = FLAGS 00 - standard packet
+ //10, 12, 14, 16, 18, 1A, 1C, 1E - failsafe packet
+ //20 - range check packet
+ packet[7] = 0;
+ packet[8] = 0;
+ //
+ if ( lpass & 1 )
+ startChan += 8 ;
+
+ for(uint8_t i = 0; i <12 ; i+=3)
+ {//12 bytes
+ chan_0 = scaleForPXX(startChan);
+ if(lpass & 1 )
+ chan_0+=2048;
+ startChan+=1;
+ //
+ chan_1 = scaleForPXX(startChan);
+ if(lpass & 1 )
+ chan_1+= 2048;
+ startChan+=1;
+ //
+ 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 & 1 )// in X8 mode send only 8ch every 9ms
+ lpass = 0 ;
+ else
+ lpass += 1 ;
+
+ uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ;
+ for (uint8_t i=22;i>8;//high byte
+ packet[limit]=lcrc;//low byte
+}
+
+uint16_t ReadFrSkyX()
+{
+ switch(state)
+ {
+ default:
+ set_start(47);
+ CC2500_SetPower();
+ CC2500_Strobe(CC2500_SFRX);
+ //
+ frskyX_build_bind_packet();
+ CC2500_Strobe(CC2500_SIDLE);
+ CC2500_WriteData(packet, packet[0]+1);
+ if(IS_BIND_DONE_on)
+ state = FRSKY_BIND_DONE;
+ else
+ state++;
+ return 9000;
+ case FRSKY_BIND_DONE:
+ initialize_data(0);
+ hopping_frequency_no=0;
+ BIND_DONE;
+ state++;
+ break;
+ case FRSKY_DATA1:
+ if ( prev_option != option )
+ {
+ CC2500_WriteReg(CC2500_0C_FSCTRL0,option); // Frequency offset hack
+ prev_option = option ;
+ }
+ CC2500_SetTxRxMode(TX_EN);
+ set_start(hopping_frequency_no);
+ CC2500_SetPower();
+ CC2500_Strobe(CC2500_SFRX);
+ hopping_frequency_no = (hopping_frequency_no+chanskip)%47;
+ CC2500_Strobe(CC2500_SIDLE);
+ CC2500_WriteData(packet, packet[0]+1);
+ //
+// frskyX_data_frame();
+ state++;
+ return 5200;
+ case FRSKY_DATA2:
+ CC2500_SetTxRxMode(RX_EN);
+ CC2500_Strobe(CC2500_SIDLE);
+ state++;
+ return 200;
+ case FRSKY_DATA3:
+ CC2500_Strobe(CC2500_SRX);
+ state++;
+ return 3100;
+ case FRSKY_DATA4:
+ len = CC2500_ReadReg(CC2500_3B_RXBYTES | CC2500_READ_BURST) & 0x7F;
+ if (len && (len<=(0x0E + 3))) //Telemetry frame is 17
+ {
+ packet_count=0;
+ CC2500_ReadData(pkt, len);
+ #if defined TELEMETRY
+ frsky_check_telemetry(pkt,len); //check if valid telemetry packets
+ //parse telemetry packets here
+ //The same telemetry function used by FrSky(D8).
+ #endif
+ }
+ else
+ {
+ packet_count++;
+ // 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 ;
+ packet_count=0;
+ #if defined TELEMETRY
+ telemetry_lost=1;
+ #endif
+ }
+ CC2500_Strobe(CC2500_SFRX); //flush the RXFIFO
+ }
+ frskyX_data_frame();
+ if ( FrX_send_seq != 0x08 )
+ {
+ FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ;
+ }
+ state = FRSKY_DATA1;
+ return 500;
+ }
+ return 1;
+}
+
+uint16_t initFrSkyX()
+{
+ set_rx_tx_addr(MProtocol_id_master);
+ Frsky_init_hop();
+ packet_count=0;
+ while(!chanskip)
+ chanskip=random(0xfefefefe)%47;
+
+ //for test***************
+ //rx_tx_addr[3]=0xB3;
+ //rx_tx_addr[2]=0xFD;
+ //************************
+ frskyX_init();
+ //
+ if(IS_AUTOBIND_FLAG_on)
+ {
+ state = FRSKY_BIND;
+ initialize_data(1);
+ }
+ else
+ {
+ state = FRSKY_DATA1;
+ initialize_data(0);
+ }
+// seq_last_sent = 0;
+// seq_last_rcvd = 8;
+ FrX_send_seq = 0x08 ;
+ FrX_receive_seq = 0 ;
+ return 10000;
+}
#endif
\ No newline at end of file
diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino
index e24ab85..3c5b8e4 100644
--- a/Multiprotocol/Multiprotocol.ino
+++ b/Multiprotocol/Multiprotocol.ino
@@ -1,1577 +1,1582 @@
-/*********************************************************
- Multiprotocol Tx code
- by Midelic and Pascal Langer(hpnuts)
- http://www.rcgroups.com/forums/showthread.php?t=2165676
- https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/edit/master/README.md
-
- Thanks to PhracturedBlue, Hexfet, Goebish, Victzh and all protocol developers
- Ported from deviation firmware
-
- 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 .
-*/
-#include
-//#define DEBUG_TX
-//#define USE_MY_CONFIG
-#include "Multiprotocol.h"
-
-//Multiprotocol module configuration file
-#include "_Config.h"
-// Let's automatically select the board
-// if arm is selected
-#ifdef __arm__
- #define STM32_BOARD
-#endif
-
-//Personal config file
-#if defined USE_MY_CONFIG
- #include "_MyConfig.h"
-#endif
-
-#include "Pins.h"
-#include "TX_Def.h"
-#include "Validate.h"
-
-#ifndef STM32_BOARD
- #include
-#else
- #include
- #include
- #include
- #include
- #include
- HardwareTimer timer(2);
- void PPM_decode();
- void ISR_COMPB();
- extern "C"
- {
- void __irq_usart2(void);
- void __irq_usart3(void);
- }
-#endif
-
-//Global constants/variables
-uint32_t MProtocol_id;//tx id,
-uint32_t MProtocol_id_master;
-uint32_t blink=0,last_signal=0;
-//
-uint16_t counter;
-uint8_t channel;
-uint8_t packet[40];
-
-#define NUM_CHN 16
-// Servo data
-uint16_t Servo_data[NUM_CHN];
-uint8_t Servo_AUX;
-uint16_t servo_max_100,servo_min_100,servo_max_125,servo_min_125;
-uint16_t servo_mid;
-
-// Protocol variables
-uint8_t cyrfmfg_id[6];//for dsm2 and devo
-uint8_t rx_tx_addr[5];
-uint8_t rx_id[4];
-uint8_t phase;
-uint16_t bind_counter;
-uint8_t bind_phase;
-uint8_t binding_idx;
-uint16_t packet_period;
-uint8_t packet_count;
-uint8_t packet_sent;
-uint8_t packet_length;
-uint8_t hopping_frequency[50];
-uint8_t *hopping_frequency_ptr;
-uint8_t hopping_frequency_no=0;
-uint8_t rf_ch_num;
-uint8_t throttle, rudder, elevator, aileron;
-uint8_t flags;
-uint16_t crc;
-uint8_t crc8;
-uint16_t seed;
-//
-uint16_t state;
-uint8_t len;
-uint8_t RX_num;
-
-#if defined(FRSKYX_CC2500_INO) || defined(SFHSS_CC2500_INO)
- uint8_t calData[48];
-#endif
-
-#ifdef CHECK_FOR_BOOTLOADER
-uint8_t BootTimer ;
-uint8_t BootState ;
-uint8_t NotBootChecking ;
-uint8_t BootCount ;
-
-#define BOOT_WAIT_30_IDLE 0
-#define BOOT_WAIT_30_DATA 1
-#define BOOT_WAIT_20 2
-#define BOOT_READY 3
-
-#endif
-
-//Channel mapping for protocols
-const uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8, AUX9, AUX10};
-const uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
-const uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
-const uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
-
-// Mode_select variables
-uint8_t mode_select;
-uint8_t protocol_flags=0,protocol_flags2=0;
-
-// PPM variable
-volatile uint16_t PPM_data[NUM_CHN];
-
-#ifndef ORANGE_TX
-//Random variable
-volatile uint32_t gWDT_entropy=0;
-#endif
-
-//Serial protocol
-uint8_t sub_protocol;
-uint8_t protocol;
-uint8_t option;
-uint8_t cur_protocol[3];
-uint8_t prev_option;
-uint8_t prev_power=0xFD; // unused power value
-
-//Serial RX variables
-#define BAUD 100000
-#define RXBUFFER_SIZE 26
-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 29
-uint8_t pkt[MAX_PKT];//telemetry receiving packets
-#if defined(TELEMETRY)
- #ifdef INVERT_TELEMETRY
- #if not defined(ORANGE_TX) && not defined(STM32_BOARD)
- // enable bit bash for serial
- #define BASH_SERIAL 1
- #endif
- #define INVERT_SERIAL 1
- #endif
- uint8_t pass = 0;
- uint8_t pktt[MAX_PKT];//telemetry receiving packets
- #ifdef BASH_SERIAL
- // For bit-bashed serial output
- #define TXBUFFER_SIZE 128
- volatile struct t_serial_bash
- {
- uint8_t head ;
- uint8_t tail ;
- uint8_t data[TXBUFFER_SIZE] ;
- uint8_t busy ;
- uint8_t speed ;
- } SerialControl ;
- #else
- #define TXBUFFER_SIZE 64
- volatile uint8_t tx_buff[TXBUFFER_SIZE];
- volatile uint8_t tx_head=0;
- volatile uint8_t tx_tail=0;
- #endif // BASH_SERIAL
- uint8_t v_lipo1;
- uint8_t v_lipo2;
- uint8_t RX_RSSI;
- uint8_t TX_RSSI;
- uint8_t RX_LQI;
- uint8_t TX_LQI;
- uint8_t telemetry_link=0;
- uint8_t telemetry_counter=0;
- uint8_t telemetry_lost;
-#endif
-
-// Callback
-typedef uint16_t (*void_function_t) (void);//pointer to a function with no parameters which return an uint16_t integer
-void_function_t remote_callback = 0;
-
-// Init
-void setup()
-{
- // General pinout
- #ifdef ORANGE_TX
- //XMEGA
- PORTD.OUTSET = 0x17 ;
- PORTD.DIRSET = 0xB2 ;
- PORTD.DIRCLR = 0x4D ;
- PORTD.PIN0CTRL = 0x18 ;
- PORTD.PIN2CTRL = 0x18 ;
- PORTE.DIRSET = 0x01 ;
- PORTE.DIRCLR = 0x02 ;
- // Timer1 config
- // TCC1 16-bit timer, clocked at 0.5uS
- EVSYS.CH3MUX = 0x80 + 0x04 ; // Prescaler of 16
- TCC1.CTRLB = 0; TCC1.CTRLC = 0; TCC1.CTRLD = 0; TCC1.CTRLE = 0;
- TCC1.INTCTRLA = 0; TIMSK1 = 0;
- TCC1.PER = 0xFFFF ;
- TCNT1 = 0 ;
- TCC1.CTRLA = 0x0B ; // Event3 (prescale of 16)
- #elif defined STM32_BOARD
- //STM32
- afio_cfg_debug_ports(AFIO_DEBUG_NONE);
- pinMode(A7105_CSN_pin,OUTPUT);
- pinMode(CC25_CSN_pin,OUTPUT);
- pinMode(NRF_CSN_pin,OUTPUT);
- pinMode(CYRF_CSN_pin,OUTPUT);
- pinMode(CYRF_RST_pin,OUTPUT);
- pinMode(PE1_pin,OUTPUT);
- pinMode(PE2_pin,OUTPUT);
- #if defined TELEMETRY
- pinMode(TX_INV_pin,OUTPUT);
- pinMode(RX_INV_pin,OUTPUT);
- #if defined INVERT_SERIAL
- TX_INV_on;//activated inverter for both serial TX and RX signals
- RX_INV_on;
- #else
- TX_INV_off;
- RX_INV_off;
- #endif
- #endif
- pinMode(BIND_pin,INPUT_PULLUP);
- pinMode(PPM_pin,INPUT);
- pinMode(S1_pin,INPUT_PULLUP);//dial switch
- pinMode(S2_pin,INPUT_PULLUP);
- pinMode(S3_pin,INPUT_PULLUP);
- pinMode(S4_pin,INPUT_PULLUP);
- //Random pins
- pinMode(PB0, INPUT_ANALOG); // set up pin for analog input
- pinMode(PB1, INPUT_ANALOG); // set up pin for analog input
-
- //select the counter clock.
- start_timer2();//0.5us
- #else
- //ATMEGA328p
- // all inputs
- DDRB=0x00;DDRC=0x00;DDRD=0x00;
- // outputs
- SDI_output;
- SCLK_output;
- #ifdef A7105_CSN_pin
- A7105_CSN_output;
- #endif
- #ifdef CC25_CSN_pin
- CC25_CSN_output;
- #endif
- #ifdef CYRF_CSN_pin
- CYRF_RST_output;
- CYRF_CSN_output;
- #endif
- #ifdef NRF_CSN_pin
- NRF_CSN_output;
- #endif
- PE1_output;
- PE2_output;
- SERIAL_TX_output;
-
- // pullups
- MODE_DIAL1_port |= _BV(MODE_DIAL1_pin);
- MODE_DIAL2_port |= _BV(MODE_DIAL2_pin);
- MODE_DIAL3_port |= _BV(MODE_DIAL3_pin);
- MODE_DIAL4_port |= _BV(MODE_DIAL4_pin);
- BIND_port |= _BV(BIND_pin);
-
- // Timer1 config
- TCCR1A = 0;
- TCCR1B = (1 << CS11); //prescaler8, set timer1 to increment every 0.5us(16Mhz) and start timer
-
- // Random
- random_init();
- #endif
-
- // Set Chip selects
- #ifdef A7105_CSN_pin
- A7105_CSN_on;
- #endif
- #ifdef CC25_CSN_pin
- CC25_CSN_on;
- #endif
- #ifdef CYRF_CSN_pin
- CYRF_CSN_on;
- #endif
- #ifdef NRF_CSN_pin
- NRF_CSN_on;
- #endif
- // Set SPI lines
- #ifdef STM32_BOARD
- initSPI2();
- #else
- SDI_on;
- SCLK_off;
- #endif
-
- // Set servos positions
- for(uint8_t i=0;iregs->IDR)>>4)&0x0F);
- #else
- mode_select =
- ((MODE_DIAL1_ipr & _BV(MODE_DIAL1_pin)) ? 0 : 1) +
- ((MODE_DIAL2_ipr & _BV(MODE_DIAL2_pin)) ? 0 : 2) +
- ((MODE_DIAL3_ipr & _BV(MODE_DIAL3_pin)) ? 0 : 4) +
- ((MODE_DIAL4_ipr & _BV(MODE_DIAL4_pin)) ? 0 : 8);
- #endif
-
- // Update LED
- LED_off;
- LED_output;
-
- //Init RF modules
- modules_reset();
-
-#ifndef ORANGE_TX
- //Init the seed with a random value created from watchdog timer for all protocols requiring random values
- #ifdef STM32_BOARD
- randomSeed((uint32_t)analogRead(PB0) << 10 | analogRead(PB1));
- #else
- randomSeed(random_value());
- #endif
-#endif
-
- // Read or create protocol id
- MProtocol_id_master=random_id(10,false);
-
-#ifdef ENABLE_PPM
- //Protocol and interrupts initialization
- if(mode_select != MODE_SERIAL)
- { // PPM
- mode_select--;
- protocol = PPM_prot[mode_select].protocol;
- cur_protocol[1] = protocol;
- sub_protocol = PPM_prot[mode_select].sub_proto;
- RX_num = PPM_prot[mode_select].rx_num;
- option = PPM_prot[mode_select].option;
- if(PPM_prot[mode_select].power) POWER_FLAG_on;
- if(PPM_prot[mode_select].autobind) AUTOBIND_FLAG_on;
- mode_select++;
- servo_max_100=PPM_MAX_100; servo_min_100=PPM_MIN_100;
- servo_max_125=PPM_MAX_125; servo_min_125=PPM_MIN_125;
-
- protocol_init();
-
- #ifndef STM32_BOARD
- //Configure PPM interrupt
- #if PPM_pin == 2
- EICRA |= _BV(ISC01); // The rising edge of INT0 pin D2 generates an interrupt request
- EIMSK |= _BV(INT0); // INT0 interrupt enable
- #elif PPM_pin == 3
- EICRA |= _BV(ISC11); // The rising edge of INT1 pin D3 generates an interrupt request
- EIMSK |= _BV(INT1); // INT1 interrupt enable
- #else
- #error PPM pin can only be 2 or 3
- #endif
- #else
- attachInterrupt(PPM_pin,PPM_decode,FALLING);
- #endif
-
- #if defined(TELEMETRY)
- PPM_Telemetry_serial_init();// Configure serial for telemetry
- #endif
- }
- else
-#endif //ENABLE_PPM
- { // Serial
- #ifdef ENABLE_SERIAL
- for(uint8_t i=0;i<3;i++)
- cur_protocol[i]=0;
- protocol=0;
- servo_max_100=SERIAL_MAX_100; servo_min_100=SERIAL_MIN_100;
- servo_max_125=SERIAL_MAX_125; servo_min_125=SERIAL_MIN_125;
-#ifdef CHECK_FOR_BOOTLOADER
- Mprotocol_serial_init(1); // Configure serial and enable RX interrupt
-#else
- Mprotocol_serial_init(); // Configure serial and enable RX interrupt
-#endif
- #endif //ENABLE_SERIAL
- }
- servo_mid=servo_min_100+servo_max_100; //In fact 2* mid_value
-}
-
-
-
-#ifdef CHECK_FOR_BOOTLOADER
-void pollBoot()
-{
- uint8_t rxchar ;
- uint8_t lState = BootState ;
- uint8_t millisTime = millis() ; // Call this once only
-#ifdef ORANGE_TX
- if ( USARTC0.STATUS & USART_RXCIF_bm )
-#elif defined STM32_BOARD
- if ( USART2_BASE->SR & USART_SR_RXNE )
-#else
- if ( UCSR0A & ( 1 << RXC0 ) )
-#endif
- {
- rxchar = UDR0 ;
- BootCount += 1 ;
- if ( ( lState == BOOT_WAIT_30_IDLE ) || ( lState == BOOT_WAIT_30_DATA ) )
- {
- if ( lState == BOOT_WAIT_30_IDLE ) // Waiting for 0x30
- {
- BootTimer = millisTime ; // Start timeout
- }
- if ( rxchar == 0x30 )
- {
- lState = BOOT_WAIT_20 ;
- }
- else
- {
- lState = BOOT_WAIT_30_DATA ;
- }
- }
- else if ( lState == BOOT_WAIT_20 ) // Waiting for 0x20
- {
- if ( rxchar == 0x20 )
- {
- lState = BOOT_READY ;
- }
- }
- }
- else // No byte received
- {
- if ( lState != BOOT_WAIT_30_IDLE ) // Something received
- {
- uint8_t time = millisTime - BootTimer ;
- if ( time > 5 )
- {
-#ifdef STM32_BOARD
- if ( BootCount > 4 )
-#else
- if ( BootCount > 2 )
-#endif
- { // Run normally
- NotBootChecking = 0xFF ;
- Mprotocol_serial_init( 0 ) ;
- }
- else if ( lState == BOOT_READY )
- {
-#ifdef STM32_BOARD
-#define SCS_BASE (0xE000E000) /*!< System Control Space Base Address */
-#define SCB_BASE (SCS_BASE + 0x0D00) /*!< System Control Block Base Address */
-#define SCB ((SCB_Type *) SCB_BASE) /*!< SCB configuration struct */
-#define __I volatile /*!< defines 'read only' permissions */
-#define __IO volatile /*!< defines 'read / write' permissions */
-typedef struct
-{
- __I uint32_t CPUID; /*!< Offset: 0x00 CPU ID Base Register */
- __IO uint32_t ICSR; /*!< Offset: 0x04 Interrupt Control State Register */
- __IO uint32_t VTOR; /*!< Offset: 0x08 Vector Table Offset Register */
- __IO uint32_t AIRCR; /*!< Offset: 0x0C Application Interrupt / Reset Control Register */
- __IO uint32_t SCR; /*!< Offset: 0x10 System Control Register */
- __IO uint32_t CCR; /*!< Offset: 0x14 Configuration Control Register */
- __IO uint8_t SHP[12]; /*!< Offset: 0x18 System Handlers Priority Registers (4-7, 8-11, 12-15) */
- __IO uint32_t SHCSR; /*!< Offset: 0x24 System Handler Control and State Register */
- __IO uint32_t CFSR; /*!< Offset: 0x28 Configurable Fault Status Register */
- __IO uint32_t HFSR; /*!< Offset: 0x2C Hard Fault Status Register */
- __IO uint32_t DFSR; /*!< Offset: 0x30 Debug Fault Status Register */
- __IO uint32_t MMFAR; /*!< Offset: 0x34 Mem Manage Address Register */
- __IO uint32_t BFAR; /*!< Offset: 0x38 Bus Fault Address Register */
- __IO uint32_t AFSR; /*!< Offset: 0x3C Auxiliary Fault Status Register */
- __I uint32_t PFR[2]; /*!< Offset: 0x40 Processor Feature Register */
- __I uint32_t DFR; /*!< Offset: 0x48 Debug Feature Register */
- __I uint32_t ADR; /*!< Offset: 0x4C Auxiliary Feature Register */
- __I uint32_t MMFR[4]; /*!< Offset: 0x50 Memory Model Feature Register */
- __I uint32_t ISAR[5]; /*!< Offset: 0x60 ISA Feature Register */
-} SCB_Type;
-#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */
-#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */
-#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */
-#define SCB_AIRCR_PRIGROUP_Msk (7ul << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */
-#define SCB_AIRCR_SYSRESETREQ_Msk (1ul << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */
-
-// NVIC_SystemReset() ;
-//static __INLINE void NVIC_SystemReset(void)
- {
- SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |
- (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
- SCB_AIRCR_SYSRESETREQ_Msk) ; /* Keep priority group unchanged */
- asm("dsb");
- while(1) ; /* wait until reset */
- }
-#else
- cli(); // Disable global int due to RW of 16 bits registers
- void (*p)() ;
-#ifndef ORANGE_TX
- p = (void (*)())0x3F00 ; // Word address (0x7E00 byte)
-#else
- p = (void (*)())0x4000 ; // Word address (0x8000 byte)
-#endif
- (*p)() ;
- // go to boot
-#endif
- }
- else
- {
- lState = BOOT_WAIT_30_IDLE ;
- BootCount = 0 ;
- }
- }
- }
- }
- BootState = lState ;
-}
-#endif
-
-// Main
-// Protocol scheduler
-void loop()
-{
- uint16_t next_callback,diff=0xFFFF;
-
- while(1)
- {
- if(remote_callback==0 || IS_WAIT_BIND_on || diff>2*200)
- {
- do
- {
- Update_All();
- }
- while(remote_callback==0 || IS_WAIT_BIND_on);
- }
- #ifndef STM32_BOARD
- if( (TIFR1 & OCF1A_bm) != 0)
- {
- cli(); // Disable global int due to RW of 16 bits registers
- OCR1A=TCNT1; // Callback should already have been called... Use "now" as new sync point.
- sei(); // Enable global int
- }
- else
- while((TIFR1 & OCF1A_bm) == 0); // Wait before callback
- #else
- if((TIMER2_BASE->SR & TIMER_SR_CC1IF)!=0)
- {
- cli();
- OCR1A = TCNT1;
- sei();
- }
- else
- while((TIMER2_BASE->SR & TIMER_SR_CC1IF )==0); // Wait before callback
- #endif
- do
- {
- TX_MAIN_PAUSE_on;
- tx_pause();
- if(IS_INPUT_SIGNAL_on && remote_callback!=0)
- next_callback=remote_callback();
- else
- next_callback=2000; // No PPM/serial signal check again in 2ms...
- TX_MAIN_PAUSE_off;
- tx_resume();
- while(next_callback>4000)
- { // start to wait here as much as we can...
- next_callback-=2000; // We will wait below for 2ms
- cli(); // Disable global int due to RW of 16 bits registers
- OCR1A += 2000*2 ; // set compare A for callback
- #ifndef STM32_BOARD
- TIFR1=OCF1A_bm; // clear compare A=callback flag
- #else
- TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag
- #endif
- sei(); // enable global int
- if(Update_All()) // Protocol changed?
- {
- next_callback=0; // Launch new protocol ASAP
- break;
- }
- #ifndef STM32_BOARD
- while((TIFR1 & OCF1A_bm) == 0); // wait 2ms...
- #else
- while((TIMER2_BASE->SR & TIMER_SR_CC1IF)==0);//2ms wait
- #endif
- }
- // at this point we have a maximum of 4ms in next_callback
- next_callback *= 2 ;
- cli(); // Disable global int due to RW of 16 bits registers
- OCR1A+= next_callback ; // set compare A for callback
- #ifndef STM32_BOARD
- TIFR1=OCF1A_bm; // clear compare A=callback flag
- #else
- TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag write zero
- #endif
- diff=OCR1A-TCNT1; // compare timer and comparator
- sei(); // enable global int
- }
- while(diff&0x8000); // Callback did not took more than requested time for next callback
- // so we can launch Update_All before next callback
- }
-}
-
-uint8_t Update_All()
-{
- #ifdef ENABLE_SERIAL
-#ifdef CHECK_FOR_BOOTLOADER
- if ( (mode_select==MODE_SERIAL) && (NotBootChecking == 0) )
- {
- pollBoot() ;
- }
- else
-#endif
- if(mode_select==MODE_SERIAL && IS_RX_FLAG_on) // Serial mode and something has been received
- {
- update_serial_data(); // Update protocol and data
- update_channels_aux();
- INPUT_SIGNAL_on; //valid signal received
- last_signal=millis();
- }
- #endif //ENABLE_SERIAL
- #ifdef ENABLE_PPM
- if(mode_select!=MODE_SERIAL && IS_PPM_FLAG_on) // PPM mode and a full frame has been received
- {
- for(uint8_t i=0;iPPM_MAX_125) temp_ppm=PPM_MAX_125;
- Servo_data[i]= temp_ppm ;
- }
- PPM_FLAG_off; // wait for next frame before update
- update_channels_aux();
- INPUT_SIGNAL_on; //valid signal received
- last_signal=millis();
- }
- #endif //ENABLE_PPM
- update_led_status();
- #if defined(TELEMETRY)
- #if ( !( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) ) )
- if((protocol==MODE_FRSKYD) || (protocol==MODE_BAYANG) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) )
- #endif
- TelemetryUpdate();
- #endif
- #ifdef ENABLE_BIND_CH
- if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && Servo_data[BIND_CH-1]>PPM_MAX_COMMAND && Servo_data[THROTTLE]<(servo_min_100+25))
- { // Autobind is on and BIND_CH went up and Throttle is low
- CHANGE_PROTOCOL_FLAG_on; //reload protocol to rebind
- BIND_CH_PREV_on;
- }
- if(IS_BIND_CH_PREV_on && Servo_data[BIND_CH-1]2)
- bind_counter=2;
- }
- #endif //ENABLE_BIND_CH
- if(IS_CHANGE_PROTOCOL_FLAG_on)
- { // Protocol needs to be changed or relaunched for bind
- protocol_init(); //init new protocol
- return 1;
- }
- return 0;
-}
-
-// Update channels direction and Servo_AUX flags based on servo AUX positions
-static void update_channels_aux(void)
-{
- //Reverse channels direction
- #ifdef REVERSE_AILERON
- Servo_data[AILERON]=servo_mid-Servo_data[AILERON];
- #endif
- #ifdef REVERSE_ELEVATOR
- Servo_data[ELEVATOR]=servo_mid-Servo_data[ELEVATOR];
- #endif
- #ifdef REVERSE_THROTTLE
- Servo_data[THROTTLE]=servo_mid-Servo_data[THROTTLE];
- #endif
- #ifdef REVERSE_RUDDER
- Servo_data[RUDDER]=servo_mid-Servo_data[RUDDER];
- #endif
- //Calc AUX flags
- Servo_AUX=0;
- for(uint8_t i=0;i<8;i++)
- if(Servo_data[AUX1+i]>PPM_SWITCH)
- Servo_AUX|=1<70)
- INPUT_SIGNAL_off; //no valid signal (PPM or Serial) received for 70ms
- if(blinkCR1 &= ~ USART_CR1_TXEIE;
- #else
- UCSR0B &= ~_BV(UDRIE0);
- #endif
- #endif
- #endif
- #endif
-}
-
-inline void tx_resume()
-{
- #ifdef TELEMETRY
- // Resume telemetry by enabling transmitter interrupt
- if(!IS_TX_PAUSE_on)
- {
- #ifdef ORANGE_TX
- cli() ;
- USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ;
- sei() ;
- #else
- #ifndef BASH_SERIAL
- #ifdef STM32_BOARD
- USART3_BASE->CR1 |= USART_CR1_TXEIE;
- #else
- UCSR0B |= _BV(UDRIE0);
- #endif
- #else
- resumeBashSerial();
- #endif
- #endif
- }
- #endif
-}
-
-#ifdef STM32_BOARD
-void start_timer2()
-{
- // Pause the timer while we're configuring it
- timer.pause();
- TIMER2_BASE->PSC = 35; //36-1;for 72 MHZ /0.5sec/(35+1)
- TIMER2_BASE->ARR = 0xFFFF; //count till max
- timer.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
- timer.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
- // Refresh the timer's count, prescale, and overflow
- timer.refresh();
- timer.resume();
-}
-#endif
-
-// Protocol start
-static void protocol_init()
-{
- static uint16_t next_callback;
- if(IS_WAIT_BIND_off)
- {
- remote_callback = 0; // No protocol
- next_callback=0; // Default is immediate call back
- LED_off; // Led off during protocol init
- modules_reset(); // Reset all modules
-
- // reset telemetry
- #ifdef TELEMETRY
- tx_pause();
- pass=0;
- telemetry_link=0;
- telemetry_lost=1;
- #ifdef BASH_SERIAL
- TIMSK0 = 0 ; // Stop all timer 0 interrupts
- #ifdef INVERT_SERIAL
- SERIAL_TX_off;
- #else
- SERIAL_TX_on;
- #endif
- SerialControl.tail=0;
- SerialControl.head=0;
- SerialControl.busy=0;
- #else
- tx_tail=0;
- tx_head=0;
- #endif
- TX_RX_PAUSE_off;
- TX_MAIN_PAUSE_off;
- #endif
-
- //Set global ID and rx_tx_addr
- MProtocol_id = RX_num + MProtocol_id_master;
- set_rx_tx_addr(MProtocol_id);
-
- blink=millis();
-
- if(IS_BIND_BUTTON_FLAG_on)
- AUTOBIND_FLAG_on;
- if(IS_AUTOBIND_FLAG_on)
- BIND_IN_PROGRESS; // Indicates bind in progress for blinking bind led
- else
- BIND_DONE;
-
- PE1_on; //NRF24L01 antenna RF3 by default
- PE2_off; //NRF24L01 antenna RF3 by default
-
- switch(protocol) // Init the requested protocol
- {
- #ifdef A7105_INSTALLED
- #if defined(FLYSKY_A7105_INO)
- case MODE_FLYSKY:
- PE1_off; //antenna RF1
- next_callback = initFlySky();
- remote_callback = ReadFlySky;
- break;
- #endif
- #if defined(AFHDS2A_A7105_INO)
- case MODE_AFHDS2A:
- PE1_off; //antenna RF1
- next_callback = initAFHDS2A();
- remote_callback = ReadAFHDS2A;
- break;
- #endif
- #if defined(HUBSAN_A7105_INO)
- case MODE_HUBSAN:
- PE1_off; //antenna RF1
- if(IS_BIND_BUTTON_FLAG_on) random_id(10,true); // Generate new ID if bind button is pressed.
- next_callback = initHubsan();
- remote_callback = ReadHubsan;
- break;
- #endif
- #endif
- #ifdef CC2500_INSTALLED
- #if defined(FRSKYD_CC2500_INO)
- case MODE_FRSKYD:
- PE1_off; //antenna RF2
- PE2_on;
- next_callback = initFrSky_2way();
- remote_callback = ReadFrSky_2way;
- break;
- #endif
- #if defined(FRSKYV_CC2500_INO)
- case MODE_FRSKYV:
- PE1_off; //antenna RF2
- PE2_on;
- next_callback = initFRSKYV();
- remote_callback = ReadFRSKYV;
- break;
- #endif
- #if defined(FRSKYX_CC2500_INO)
- case MODE_FRSKYX:
- PE1_off; //antenna RF2
- PE2_on;
- next_callback = initFrSkyX();
- remote_callback = ReadFrSkyX;
- break;
- #endif
- #if defined(SFHSS_CC2500_INO)
- case MODE_SFHSS:
- PE1_off; //antenna RF2
- PE2_on;
- next_callback = initSFHSS();
- remote_callback = ReadSFHSS;
- break;
- #endif
- #endif
- #ifdef CYRF6936_INSTALLED
- #if defined(DSM_CYRF6936_INO)
- case MODE_DSM:
- PE2_on; //antenna RF4
- next_callback = initDsm();
- //Servo_data[2]=1500;//before binding
- remote_callback = ReadDsm;
- break;
- #endif
- #if defined(DEVO_CYRF6936_INO)
- case MODE_DEVO:
- #ifdef ENABLE_PPM
- if(mode_select) //PPM mode
- {
- if(IS_BIND_BUTTON_FLAG_on)
- {
- eeprom_write_byte((EE_ADDR)(30+mode_select),0x00); // reset to autobind mode for the current model
- option=0;
- }
- else
- {
- option=eeprom_read_byte((EE_ADDR)(30+mode_select)); // load previous mode: autobind or fixed id
- if(option!=1) option=0; // if not fixed id mode then it should be autobind
- }
- }
- #endif //ENABLE_PPM
- PE2_on; //antenna RF4
- next_callback = DevoInit();
- remote_callback = devo_callback;
- break;
- #endif
- #if defined(WK2x01_CYRF6936_INO)
- case MODE_WK2x01:
- #ifdef ENABLE_PPM
- if(mode_select) //PPM mode
- {
- if(IS_BIND_BUTTON_FLAG_on)
- {
- eeprom_write_byte((EE_ADDR)(30+mode_select),0x00); // reset to autobind mode for the current model
- option=0;
- }
- else
- {
- option=eeprom_read_byte((EE_ADDR)(30+mode_select)); // load previous mode: autobind or fixed id
- if(option!=1) option=0; // if not fixed id mode then it should be autobind
- }
- }
- #endif //ENABLE_PPM
- PE2_on; //antenna RF4
- next_callback = WK_setup();
- remote_callback = WK_cb;
- break;
- #endif
- #if defined(J6PRO_CYRF6936_INO)
- case MODE_J6PRO:
- PE2_on; //antenna RF4
- next_callback = initJ6Pro();
- remote_callback = ReadJ6Pro;
- break;
- #endif
- #endif
- #ifdef NRF24L01_INSTALLED
- #if defined(HISKY_NRF24L01_INO)
- case MODE_HISKY:
- next_callback=initHiSky();
- remote_callback = hisky_cb;
- break;
- #endif
- #if defined(V2X2_NRF24L01_INO)
- case MODE_V2X2:
- next_callback = initV2x2();
- remote_callback = ReadV2x2;
- break;
- #endif
- #if defined(YD717_NRF24L01_INO)
- case MODE_YD717:
- next_callback=initYD717();
- remote_callback = yd717_callback;
- break;
- #endif
- #if defined(KN_NRF24L01_INO)
- case MODE_KN:
- next_callback = initKN();
- remote_callback = kn_callback;
- break;
- #endif
- #if defined(SYMAX_NRF24L01_INO)
- case MODE_SYMAX:
- next_callback = initSymax();
- remote_callback = symax_callback;
- break;
- #endif
- #if defined(SLT_NRF24L01_INO)
- case MODE_SLT:
- next_callback=initSLT();
- remote_callback = SLT_callback;
- break;
- #endif
- #if defined(CX10_NRF24L01_INO)
- case MODE_Q2X2:
- sub_protocol|=0x08; // Increase the number of sub_protocols for CX-10
- case MODE_CX10:
- next_callback=initCX10();
- remote_callback = CX10_callback;
- break;
- #endif
- #if defined(CG023_NRF24L01_INO)
- case MODE_CG023:
- next_callback=initCG023();
- remote_callback = CG023_callback;
- break;
- #endif
- #if defined(BAYANG_NRF24L01_INO)
- case MODE_BAYANG:
- next_callback=initBAYANG();
- remote_callback = BAYANG_callback;
- break;
- #endif
- #if defined(ESKY_NRF24L01_INO)
- case MODE_ESKY:
- next_callback=initESKY();
- remote_callback = ESKY_callback;
- break;
- #endif
- #if defined(MT99XX_NRF24L01_INO)
- case MODE_MT99XX:
- next_callback=initMT99XX();
- remote_callback = MT99XX_callback;
- break;
- #endif
- #if defined(MJXQ_NRF24L01_INO)
- case MODE_MJXQ:
- next_callback=initMJXQ();
- remote_callback = MJXQ_callback;
- break;
- #endif
- #if defined(SHENQI_NRF24L01_INO)
- case MODE_SHENQI:
- next_callback=initSHENQI();
- remote_callback = SHENQI_callback;
- break;
- #endif
- #if defined(FY326_NRF24L01_INO)
- case MODE_FY326:
- next_callback=initFY326();
- remote_callback = FY326_callback;
- break;
- #endif
- #if defined(FQ777_NRF24L01_INO)
- case MODE_FQ777:
- next_callback=initFQ777();
- remote_callback = FQ777_callback;
- break;
- #endif
- #if defined(ASSAN_NRF24L01_INO)
- case MODE_ASSAN:
- next_callback=initASSAN();
- remote_callback = ASSAN_callback;
- break;
- #endif
- #if defined(HONTAI_NRF24L01_INO)
- case MODE_HONTAI:
- next_callback=initHONTAI();
- remote_callback = HONTAI_callback;
- break;
- #endif
- #if defined(Q303_NRF24L01_INO)
- case MODE_Q303:
- next_callback=initQ303();
- remote_callback = Q303_callback;
- break;
- #endif
- #if defined(GW008_NRF24L01_INO)
- case MODE_GW008:
- next_callback=initGW008();
- remote_callback = GW008_callback;
- break;
- #endif
- #if defined(DM002_NRF24L01_INO)
- case MODE_DM002:
- next_callback=initDM002();
- remote_callback = DM002_callback;
- break;
- #endif
- #endif
- }
- }
-
- #if defined(WAIT_FOR_BIND) && defined(ENABLE_BIND_CH)
- if( IS_AUTOBIND_FLAG_on && ! ( IS_BIND_CH_PREV_on || IS_BIND_BUTTON_FLAG_on || (cur_protocol[1]&0x80)!=0 ) )
- {
- WAIT_BIND_on;
- return;
- }
- #endif
- WAIT_BIND_off;
- CHANGE_PROTOCOL_FLAG_off;
-
- if(next_callback>32000)
- { // next_callback should not be more than 32767 so we will wait here...
- uint16_t temp=(next_callback>>10)-2;
- delayMilliseconds(temp);
- next_callback-=temp<<10; // between 2-3ms left at this stage
- }
- cli(); // disable global int
- OCR1A = TCNT1 + next_callback*2; // set compare A for callback
- sei(); // enable global int
- #ifndef STM32_BOARD
- TIFR1 = OCF1A_bm ; // clear compare A flag
- #else
- TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag write zero
- #endif
- BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change
-}
-
-void update_serial_data()
-{
- RX_DONOTUPDTAE_on;
- RX_FLAG_off; //data is being processed
- if(rx_ok_buff[1]&0x20) //check range
- RANGE_FLAG_on;
- else
- RANGE_FLAG_off;
- if(rx_ok_buff[1]&0xC0) //check autobind(0x40) & bind(0x80) together
- AUTOBIND_FLAG_on;
- else
- AUTOBIND_FLAG_off;
- if(rx_ok_buff[2]&0x80) //if rx_ok_buff[2] ==1,power is low ,0-power high
- POWER_FLAG_off; //power low
- else
- POWER_FLAG_on; //power high
-
- option=rx_ok_buff[3];
-
- 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
- WAIT_BIND_off;
- protocol=(rx_ok_buff[0]==0x55?0:32) + (rx_ok_buff[1]&0x1F); //protocol no (0-63) bits 4-6 of buff[1] and bit 0 of buf[0]
- sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6
- RX_num=rx_ok_buff[2]& 0x0F; // rx_num bits 0---3
- }
- else
- if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set
- CHANGE_PROTOCOL_FLAG_on; //restart protocol with bind
- else
- if( ((rx_ok_buff[1]&0x80)==0) && ((cur_protocol[1]&0x80)!=0) ) // Bind flag has been reset
- {
- #if defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) || defined(FRSKYV_CC2500_INO)
- if(protocol==MODE_FRSKYD || protocol==MODE_FRSKYX || protocol==MODE_FRSKYV)
- BIND_DONE;
- else
- #endif
- if(bind_counter>2)
- bind_counter=2;
- }
-
- //store current protocol values
- for(uint8_t i=0;i<3;i++)
- cur_protocol[i] = rx_ok_buff[i];
-
- // decode channel values
- volatile uint8_t *p=rx_ok_buff+3;
- uint8_t dec=-3;
- for(uint8_t i=0;i=8)
- {
- dec-=8;
- p++;
- }
- p++;
- Servo_data[i]=((((*((uint32_t *)p))>>dec)&0x7FF)*5)/8+860; //value range 860<->2140 -125%<->+125%
- }
- RX_DONOTUPDTAE_off;
- #ifdef ORANGE_TX
- cli();
- #else
- UCSR0B &= ~_BV(RXCIE0); // RX interrupt disable
- #endif
- if(IS_RX_MISSED_BUFF_on) // If the buffer is still valid
- { memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer
- RX_FLAG_on; // data to be processed next time...
- RX_MISSED_BUFF_off;
- }
- #ifdef ORANGE_TX
- sei();
- #else
- UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
- #endif
-}
-
-void modules_reset()
-{
- #ifdef CC2500_INSTALLED
- CC2500_Reset();
- #endif
- #ifdef A7105_INSTALLED
- A7105_Reset();
- #endif
- #ifdef CYRF6936_INSTALLED
- CYRF_Reset();
- #endif
- #ifdef NRF24L01_INSTALLED
- NRF24L01_Reset();
- #endif
-
- //Wait for every component to reset
- delayMilliseconds(100);
- prev_power=0xFD; // unused power value
-}
-
-#ifdef CHECK_FOR_BOOTLOADER
-void Mprotocol_serial_init( uint8_t boot )
-#else
-void Mprotocol_serial_init()
-#endif
-{
- #ifdef ORANGE_TX
- PORTC.OUTSET = 0x08 ;
- PORTC.DIRSET = 0x08 ;
-
- USARTC0.BAUDCTRLA = 19 ;
- USARTC0.BAUDCTRLB = 0 ;
-
- USARTC0.CTRLB = 0x18 ;
- USARTC0.CTRLA = (USARTC0.CTRLA & 0xCC) | 0x11 ;
- USARTC0.CTRLC = 0x2B ;
- UDR0 ;
- #ifdef INVERT_SERIAL
- PORTC.PIN3CTRL |= 0x40 ;
- #endif
-#ifdef CHECK_FOR_BOOTLOADER
- if ( boot )
- {
- USARTC0.BAUDCTRLB = 0 ;
- USARTC0.BAUDCTRLA = 33 ; // 57600
- USARTC0.CTRLA = (USARTC0.CTRLA & 0xC0) ;
- USARTC0.CTRLC = 0x03 ; // 8 bit, no parity, 1 stop
- USARTC0.CTRLB = 0x18 ; // Enable Tx and Rx
- PORTC.PIN3CTRL &= ~0x40 ;
- }
-#endif // CHECK_FOR_BOOTLOADER
- #elif defined STM32_BOARD
-#ifdef CHECK_FOR_BOOTLOADER
- if ( boot )
- {
- usart2_begin(57600,SERIAL_8N1);
- USART2_BASE->CR1 &= ~USART_CR1_RXNEIE ;
- (void)UDR0 ;
- }
- else
- {
- usart2_begin(100000,SERIAL_8E2);
- USART2_BASE->CR1 |= USART_CR1_PCE_BIT;
- }
-#else
- usart2_begin(100000,SERIAL_8E2);
- USART2_BASE->CR1 |= USART_CR1_PCE_BIT;
-#endif // CHECK_FOR_BOOTLOADER
- usart3_begin(100000,SERIAL_8E2);
- USART3_BASE->CR1 &= ~ USART_CR1_RE;//disable
- USART2_BASE->CR1 &= ~ USART_CR1_TE;//disable transmit
- #else
- //ATMEGA328p
- #include
- UBRR0H = UBRRH_VALUE;
- UBRR0L = UBRRL_VALUE;
- UCSR0A = 0 ; // Clear X2 bit
- //Set frame format to 8 data bits, even parity, 2 stop bits
- UCSR0C = _BV(UPM01)|_BV(USBS0)|_BV(UCSZ01)|_BV(UCSZ00);
- while ( UCSR0A & (1 << RXC0) )//flush receive buffer
- UDR0;
- //enable reception and RC complete interrupt
- UCSR0B = _BV(RXEN0)|_BV(RXCIE0);//rx enable and interrupt
- #ifndef DEBUG_TX
- #if defined(TELEMETRY)
- initTXSerial( SPEED_100K ) ;
- #endif //TELEMETRY
- #endif //DEBUG_TX
-#ifdef CHECK_FOR_BOOTLOADER
- if ( boot )
- {
- UBRR0H = 0 ;
- UBRR0L = 33 ; // 57600
- UCSR0C &= ~_BV(UPM01) ; // No parity
- UCSR0B &= ~_BV(RXCIE0);// No rx interrupt
- UCSR0A |= _BV(U2X0); //Double speed mode USART0
- }
-#endif // CHECK_FOR_BOOTLOADER
- #endif //ORANGE_TX
-}
-
-#if defined(TELEMETRY)
-void PPM_Telemetry_serial_init()
-{
- if( (protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_BAYANG) )
- initTXSerial( SPEED_9600 ) ;
- if(protocol==MODE_FRSKYX)
- initTXSerial( SPEED_57600 ) ;
- if(protocol==MODE_DSM)
- initTXSerial( SPEED_125K ) ;
-}
-#endif
-
-// Convert 32b id to rx_tx_addr
-static void set_rx_tx_addr(uint32_t id)
-{ // Used by almost all protocols
- rx_tx_addr[0] = (id >> 24) & 0xFF;
- rx_tx_addr[1] = (id >> 16) & 0xFF;
- rx_tx_addr[2] = (id >> 8) & 0xFF;
- rx_tx_addr[3] = (id >> 0) & 0xFF;
- rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F);
-}
-
-#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
-static void random_init(void)
-{
- cli(); // Temporarily turn off interrupts, until WDT configured
- MCUSR = 0; // Use the MCU status register to reset flags for WDR, BOR, EXTR, and POWR
- WDTCSR |= _BV(WDCE); // WDT control register, This sets the Watchdog Change Enable (WDCE) flag, which is needed to set the prescaler
- WDTCSR = _BV(WDIE); // Watchdog interrupt enable (WDIE)
- sei(); // Turn interupts on
-}
-
-static uint32_t random_value(void)
-{
- while (!gWDT_entropy);
- return gWDT_entropy;
-}
-#endif
-
-static uint32_t random_id(uint16_t address, uint8_t create_new)
-{
- #ifndef FORCE_GLOBAL_ID
- uint32_t id=0;
-
- if(eeprom_read_byte((EE_ADDR)(address+10))==0xf0 && !create_new)
- { // TXID exists in EEPROM
- for(uint8_t i=4;i>0;i--)
- {
- id<<=8;
- id|=eeprom_read_byte((EE_ADDR)address+i-1);
- }
- if(id!=0x2AD141A7) //ID with seed=0
- return id;
- }
- // Generate a random ID
- #if defined STM32_BOARD
- #define STM32_UUID ((uint32_t *)0x1FFFF7E8)
- if (!create_new)
- id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2];
- #else
- id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
- #endif
- for(uint8_t i=0;i<4;i++)
- {
- eeprom_write_byte((EE_ADDR)address+i,id);
- id>>=8;
- }
- eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom.
- return id;
- #else
- (void)address;
- (void)create_new;
- return FORCE_GLOBAL_ID;
- #endif
-}
-
-/**************************/
-/**************************/
-/** Interrupt routines **/
-/**************************/
-/**************************/
-
-//PPM
-#ifdef ENABLE_PPM
- #ifdef ORANGE_TX
- #if PPM_pin == 2
- ISR(PORTD_INT0_vect)
- #else
- ISR(PORTD_INT1_vect)
- #endif
- #elif defined STM32_BOARD
- void PPM_decode()
- #else
- #if PPM_pin == 2
- ISR(INT0_vect, ISR_NOBLOCK)
- #else
- ISR(INT1_vect, ISR_NOBLOCK)
- #endif
- #endif
- { // Interrupt on PPM pin
- static int8_t chan=0,bad_frame=1;
- static uint16_t Prev_TCNT1=0;
- uint16_t Cur_TCNT1;
-
- Cur_TCNT1 = TCNT1 - Prev_TCNT1 ; // Capture current Timer1 value
- if(Cur_TCNT1<1000)
- bad_frame=1; // bad frame
- else
- if(Cur_TCNT1>4840)
- { //start of frame
- if(chan>=MIN_PPM_CHANNELS)
- PPM_FLAG_on; // good frame received if at least 4 channels have been seen
- chan=0; // reset channel counter
- bad_frame=0;
- }
- else
- if(bad_frame==0) // need to wait for start of frame
- { //servo values between 500us and 2420us will end up here
- PPM_data[chan]= Cur_TCNT1>>1;;
- if(chan++>=MAX_PPM_CHANNELS)
- bad_frame=1; // don't accept any new channels
- }
- Prev_TCNT1+=Cur_TCNT1;
- }
-#endif //ENABLE_PPM
-
-//Serial RX
-#ifdef ENABLE_SERIAL
- #ifdef ORANGE_TX
- ISR(USARTC0_RXC_vect)
- #elif defined STM32_BOARD
- void __irq_usart2()
- #else
- ISR(USART_RX_vect)
- #endif
- { // RX interrupt
- static uint8_t idx=0;
- #ifdef ORANGE_TX
- if((USARTC0.STATUS & 0x1C)==0) // Check frame error, data overrun and parity error
- #elif defined STM32_BOARD
- if((USART2_BASE->SR & USART_SR_RXNE) && (USART2_BASE->SR &0x0F)==0)
- #else
- UCSR0B &= ~_BV(RXCIE0) ; // RX interrupt disable
- sei() ;
- if((UCSR0A&0x1C)==0) // Check frame error, data overrun and parity error
- #endif
- { // received byte is ok to process
- if(idx==0||discard_frame==1)
- { // Let's try to sync at this point
- idx=0;discard_frame=0;
- RX_MISSED_BUFF_off; // If rx_buff was good it's not anymore...
- rx_buff[0]=UDR0;
- if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
- {
- TX_RX_PAUSE_on;
- tx_pause();
- #if defined STM32_BOARD
- uint16_t OCR1B;
- OCR1B =TCNT1+(6500L);
- timer.setCompare(TIMER_CH2,OCR1B);
- timer.attachCompare2Interrupt(ISR_COMPB);
- #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
- #endif
- idx++;
- }
- }
- else
- {
- rx_buff[idx++]=UDR0; // Store received byte
- if(idx>=RXBUFFER_SIZE)
- { // A full frame has been received
- if(!IS_RX_DONOTUPDTAE_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
- RX_FLAG_on; // flag for main to process servo data
- }
- else
- RX_MISSED_BUFF_on; // notify that rx_buff is good
- discard_frame=1; // start again
- }
- }
- }
- else
- {
- idx=UDR0; // Dummy read
- discard_frame=1; // Error encountered discard full frame...
- }
- if(discard_frame==1)
- {
- #ifdef STM32_BOARD
- detachInterrupt(2); // Disable interrupt on ch2
- #else
- CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
- #endif
- TX_RX_PAUSE_off;
- tx_resume();
- }
- #if not defined (ORANGE_TX) && not defined (STM32_BOARD)
- cli() ;
- UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
- #endif
- }
-
- //Serial timer
- #ifdef ORANGE_TX
- ISR(TCC1_CCB_vect)
- #elif defined STM32_BOARD
- void ISR_COMPB()
- #else
- ISR(TIMER1_COMPB_vect, ISR_NOBLOCK )
- #endif
- { // Timer1 compare B interrupt
- discard_frame=1;
- #ifdef STM32_BOARD
- detachInterrupt(2); // Disable interrupt on ch2
- #else
- CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
- #endif
- tx_resume();
- }
-#endif //ENABLE_SERIAL
-
-#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
- // Random interrupt service routine called every time the WDT interrupt is triggered.
- // It is only enabled at startup to generate a seed.
- ISR(WDT_vect)
- {
- static uint8_t gWDT_buffer_position=0;
- #define gWDT_buffer_SIZE 32
- static uint8_t gWDT_buffer[gWDT_buffer_SIZE];
- gWDT_buffer[gWDT_buffer_position] = TCNT1L; // Record the Timer 1 low byte (only one needed)
- gWDT_buffer_position++; // every time the WDT interrupt is triggered
- if (gWDT_buffer_position >= gWDT_buffer_SIZE)
- {
- // The following code is an implementation of Jenkin's one at a time hash
- for(uint8_t gWDT_loop_counter = 0; gWDT_loop_counter < gWDT_buffer_SIZE; ++gWDT_loop_counter)
- {
- gWDT_entropy += gWDT_buffer[gWDT_loop_counter];
- gWDT_entropy += (gWDT_entropy << 10);
- gWDT_entropy ^= (gWDT_entropy >> 6);
- }
- gWDT_entropy += (gWDT_entropy << 3);
- gWDT_entropy ^= (gWDT_entropy >> 11);
- gWDT_entropy += (gWDT_entropy << 15);
- WDTCSR = 0; // Disable Watchdog interrupt
- }
- }
-#endif
+/*********************************************************
+ Multiprotocol Tx code
+ by Midelic and Pascal Langer(hpnuts)
+ http://www.rcgroups.com/forums/showthread.php?t=2165676
+ https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/edit/master/README.md
+
+ Thanks to PhracturedBlue, Hexfet, Goebish, Victzh and all protocol developers
+ Ported from deviation firmware
+
+ 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 .
+*/
+#include
+//#define DEBUG_TX
+//#define USE_MY_CONFIG
+
+#ifdef ARDUINO_AVR_XMEGA32D4
+#include "MultiOrange.h"
+#endif
+
+#include "Multiprotocol.h"
+
+//Multiprotocol module configuration file
+#include "_Config.h"
+// Let's automatically select the board
+// if arm is selected
+#ifdef __arm__
+ #define STM32_BOARD
+#endif
+
+//Personal config file
+#if defined USE_MY_CONFIG
+ #include "_MyConfig.h"
+#endif
+
+#include "Pins.h"
+#include "TX_Def.h"
+#include "Validate.h"
+
+#ifndef STM32_BOARD
+ #include
+#else
+ #include
+ #include
+ #include
+ #include
+ #include
+ HardwareTimer timer(2);
+ void PPM_decode();
+ void ISR_COMPB();
+ extern "C"
+ {
+ void __irq_usart2(void);
+ void __irq_usart3(void);
+ }
+#endif
+
+//Global constants/variables
+uint32_t MProtocol_id;//tx id,
+uint32_t MProtocol_id_master;
+uint32_t blink=0,last_signal=0;
+//
+uint16_t counter;
+uint8_t channel;
+uint8_t packet[40];
+
+#define NUM_CHN 16
+// Servo data
+uint16_t Servo_data[NUM_CHN];
+uint8_t Servo_AUX;
+uint16_t servo_max_100,servo_min_100,servo_max_125,servo_min_125;
+uint16_t servo_mid;
+
+// Protocol variables
+uint8_t cyrfmfg_id[6];//for dsm2 and devo
+uint8_t rx_tx_addr[5];
+uint8_t rx_id[4];
+uint8_t phase;
+uint16_t bind_counter;
+uint8_t bind_phase;
+uint8_t binding_idx;
+uint16_t packet_period;
+uint8_t packet_count;
+uint8_t packet_sent;
+uint8_t packet_length;
+uint8_t hopping_frequency[50];
+uint8_t *hopping_frequency_ptr;
+uint8_t hopping_frequency_no=0;
+uint8_t rf_ch_num;
+uint8_t throttle, rudder, elevator, aileron;
+uint8_t flags;
+uint16_t crc;
+uint8_t crc8;
+uint16_t seed;
+//
+uint16_t state;
+uint8_t len;
+uint8_t RX_num;
+
+#if defined(FRSKYX_CC2500_INO) || defined(SFHSS_CC2500_INO)
+ uint8_t calData[48];
+#endif
+
+#ifdef CHECK_FOR_BOOTLOADER
+uint8_t BootTimer ;
+uint8_t BootState ;
+uint8_t NotBootChecking ;
+uint8_t BootCount ;
+
+#define BOOT_WAIT_30_IDLE 0
+#define BOOT_WAIT_30_DATA 1
+#define BOOT_WAIT_20 2
+#define BOOT_READY 3
+
+#endif
+
+//Channel mapping for protocols
+const uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8, AUX9, AUX10};
+const uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
+const uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
+const uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, AUX1, AUX2, AUX3, AUX4, AUX5, AUX6, AUX7, AUX8};
+
+// Mode_select variables
+uint8_t mode_select;
+uint8_t protocol_flags=0,protocol_flags2=0;
+
+// PPM variable
+volatile uint16_t PPM_data[NUM_CHN];
+
+#ifndef ORANGE_TX
+//Random variable
+volatile uint32_t gWDT_entropy=0;
+#endif
+
+//Serial protocol
+uint8_t sub_protocol;
+uint8_t protocol;
+uint8_t option;
+uint8_t cur_protocol[3];
+uint8_t prev_option;
+uint8_t prev_power=0xFD; // unused power value
+
+//Serial RX variables
+#define BAUD 100000
+#define RXBUFFER_SIZE 26
+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 29
+uint8_t pkt[MAX_PKT];//telemetry receiving packets
+#if defined(TELEMETRY)
+ #ifdef INVERT_TELEMETRY
+ #if not defined(ORANGE_TX) && not defined(STM32_BOARD)
+ // enable bit bash for serial
+ #define BASH_SERIAL 1
+ #endif
+ #define INVERT_SERIAL 1
+ #endif
+ uint8_t pass = 0;
+ uint8_t pktt[MAX_PKT];//telemetry receiving packets
+ #ifdef BASH_SERIAL
+ // For bit-bashed serial output
+ #define TXBUFFER_SIZE 192
+ volatile struct t_serial_bash
+ {
+ uint8_t head ;
+ uint8_t tail ;
+ uint8_t data[TXBUFFER_SIZE] ;
+ uint8_t busy ;
+ uint8_t speed ;
+ } SerialControl ;
+ #else
+ #define TXBUFFER_SIZE 96
+ volatile uint8_t tx_buff[TXBUFFER_SIZE];
+ volatile uint8_t tx_head=0;
+ volatile uint8_t tx_tail=0;
+ #endif // BASH_SERIAL
+ uint8_t v_lipo1;
+ uint8_t v_lipo2;
+ uint8_t RX_RSSI;
+ uint8_t TX_RSSI;
+ uint8_t RX_LQI;
+ uint8_t TX_LQI;
+ uint8_t telemetry_link=0;
+ uint8_t telemetry_counter=0;
+ uint8_t telemetry_lost;
+#endif
+
+// Callback
+typedef uint16_t (*void_function_t) (void);//pointer to a function with no parameters which return an uint16_t integer
+void_function_t remote_callback = 0;
+
+// Init
+void setup()
+{
+ // General pinout
+ #ifdef ORANGE_TX
+ //XMEGA
+ PORTD.OUTSET = 0x17 ;
+ PORTD.DIRSET = 0xB2 ;
+ PORTD.DIRCLR = 0x4D ;
+ PORTD.PIN0CTRL = 0x18 ;
+ PORTD.PIN2CTRL = 0x18 ;
+ PORTE.DIRSET = 0x01 ;
+ PORTE.DIRCLR = 0x02 ;
+ // Timer1 config
+ // TCC1 16-bit timer, clocked at 0.5uS
+ EVSYS.CH3MUX = 0x80 + 0x04 ; // Prescaler of 16
+ TCC1.CTRLB = 0; TCC1.CTRLC = 0; TCC1.CTRLD = 0; TCC1.CTRLE = 0;
+ TCC1.INTCTRLA = 0; TIMSK1 = 0;
+ TCC1.PER = 0xFFFF ;
+ TCNT1 = 0 ;
+ TCC1.CTRLA = 0x0B ; // Event3 (prescale of 16)
+ #elif defined STM32_BOARD
+ //STM32
+ afio_cfg_debug_ports(AFIO_DEBUG_NONE);
+ pinMode(A7105_CSN_pin,OUTPUT);
+ pinMode(CC25_CSN_pin,OUTPUT);
+ pinMode(NRF_CSN_pin,OUTPUT);
+ pinMode(CYRF_CSN_pin,OUTPUT);
+ pinMode(CYRF_RST_pin,OUTPUT);
+ pinMode(PE1_pin,OUTPUT);
+ pinMode(PE2_pin,OUTPUT);
+ #if defined TELEMETRY
+ pinMode(TX_INV_pin,OUTPUT);
+ pinMode(RX_INV_pin,OUTPUT);
+ #if defined INVERT_SERIAL
+ TX_INV_on;//activated inverter for both serial TX and RX signals
+ RX_INV_on;
+ #else
+ TX_INV_off;
+ RX_INV_off;
+ #endif
+ #endif
+ pinMode(BIND_pin,INPUT_PULLUP);
+ pinMode(PPM_pin,INPUT);
+ pinMode(S1_pin,INPUT_PULLUP);//dial switch
+ pinMode(S2_pin,INPUT_PULLUP);
+ pinMode(S3_pin,INPUT_PULLUP);
+ pinMode(S4_pin,INPUT_PULLUP);
+ //Random pins
+ pinMode(PB0, INPUT_ANALOG); // set up pin for analog input
+ pinMode(PB1, INPUT_ANALOG); // set up pin for analog input
+
+ //select the counter clock.
+ start_timer2();//0.5us
+ #else
+ //ATMEGA328p
+ // all inputs
+ DDRB=0x00;DDRC=0x00;DDRD=0x00;
+ // outputs
+ SDI_output;
+ SCLK_output;
+ #ifdef A7105_CSN_pin
+ A7105_CSN_output;
+ #endif
+ #ifdef CC25_CSN_pin
+ CC25_CSN_output;
+ #endif
+ #ifdef CYRF_CSN_pin
+ CYRF_RST_output;
+ CYRF_CSN_output;
+ #endif
+ #ifdef NRF_CSN_pin
+ NRF_CSN_output;
+ #endif
+ PE1_output;
+ PE2_output;
+ SERIAL_TX_output;
+
+ // pullups
+ MODE_DIAL1_port |= _BV(MODE_DIAL1_pin);
+ MODE_DIAL2_port |= _BV(MODE_DIAL2_pin);
+ MODE_DIAL3_port |= _BV(MODE_DIAL3_pin);
+ MODE_DIAL4_port |= _BV(MODE_DIAL4_pin);
+ BIND_port |= _BV(BIND_pin);
+
+ // Timer1 config
+ TCCR1A = 0;
+ TCCR1B = (1 << CS11); //prescaler8, set timer1 to increment every 0.5us(16Mhz) and start timer
+
+ // Random
+ random_init();
+ #endif
+
+ // Set Chip selects
+ #ifdef A7105_CSN_pin
+ A7105_CSN_on;
+ #endif
+ #ifdef CC25_CSN_pin
+ CC25_CSN_on;
+ #endif
+ #ifdef CYRF_CSN_pin
+ CYRF_CSN_on;
+ #endif
+ #ifdef NRF_CSN_pin
+ NRF_CSN_on;
+ #endif
+ // Set SPI lines
+ #ifdef STM32_BOARD
+ initSPI2();
+ #else
+ SDI_on;
+ SCLK_off;
+ #endif
+
+ // Set servos positions
+ for(uint8_t i=0;iregs->IDR)>>4)&0x0F);
+ #else
+ mode_select =
+ ((MODE_DIAL1_ipr & _BV(MODE_DIAL1_pin)) ? 0 : 1) +
+ ((MODE_DIAL2_ipr & _BV(MODE_DIAL2_pin)) ? 0 : 2) +
+ ((MODE_DIAL3_ipr & _BV(MODE_DIAL3_pin)) ? 0 : 4) +
+ ((MODE_DIAL4_ipr & _BV(MODE_DIAL4_pin)) ? 0 : 8);
+ #endif
+
+ // Update LED
+ LED_off;
+ LED_output;
+
+ //Init RF modules
+ modules_reset();
+
+#ifndef ORANGE_TX
+ //Init the seed with a random value created from watchdog timer for all protocols requiring random values
+ #ifdef STM32_BOARD
+ randomSeed((uint32_t)analogRead(PB0) << 10 | analogRead(PB1));
+ #else
+ randomSeed(random_value());
+ #endif
+#endif
+
+ // Read or create protocol id
+ MProtocol_id_master=random_id(10,false);
+
+#ifdef ENABLE_PPM
+ //Protocol and interrupts initialization
+ if(mode_select != MODE_SERIAL)
+ { // PPM
+ mode_select--;
+ protocol = PPM_prot[mode_select].protocol;
+ cur_protocol[1] = protocol;
+ sub_protocol = PPM_prot[mode_select].sub_proto;
+ RX_num = PPM_prot[mode_select].rx_num;
+ option = PPM_prot[mode_select].option;
+ if(PPM_prot[mode_select].power) POWER_FLAG_on;
+ if(PPM_prot[mode_select].autobind) AUTOBIND_FLAG_on;
+ mode_select++;
+ servo_max_100=PPM_MAX_100; servo_min_100=PPM_MIN_100;
+ servo_max_125=PPM_MAX_125; servo_min_125=PPM_MIN_125;
+
+ protocol_init();
+
+ #ifndef STM32_BOARD
+ //Configure PPM interrupt
+ #if PPM_pin == 2
+ EICRA |= _BV(ISC01); // The rising edge of INT0 pin D2 generates an interrupt request
+ EIMSK |= _BV(INT0); // INT0 interrupt enable
+ #elif PPM_pin == 3
+ EICRA |= _BV(ISC11); // The rising edge of INT1 pin D3 generates an interrupt request
+ EIMSK |= _BV(INT1); // INT1 interrupt enable
+ #else
+ #error PPM pin can only be 2 or 3
+ #endif
+ #else
+ attachInterrupt(PPM_pin,PPM_decode,FALLING);
+ #endif
+
+ #if defined(TELEMETRY)
+ PPM_Telemetry_serial_init();// Configure serial for telemetry
+ #endif
+ }
+ else
+#endif //ENABLE_PPM
+ { // Serial
+ #ifdef ENABLE_SERIAL
+ for(uint8_t i=0;i<3;i++)
+ cur_protocol[i]=0;
+ protocol=0;
+ servo_max_100=SERIAL_MAX_100; servo_min_100=SERIAL_MIN_100;
+ servo_max_125=SERIAL_MAX_125; servo_min_125=SERIAL_MIN_125;
+#ifdef CHECK_FOR_BOOTLOADER
+ Mprotocol_serial_init(1); // Configure serial and enable RX interrupt
+#else
+ Mprotocol_serial_init(); // Configure serial and enable RX interrupt
+#endif
+ #endif //ENABLE_SERIAL
+ }
+ servo_mid=servo_min_100+servo_max_100; //In fact 2* mid_value
+}
+
+
+
+#ifdef CHECK_FOR_BOOTLOADER
+void pollBoot()
+{
+ uint8_t rxchar ;
+ uint8_t lState = BootState ;
+ uint8_t millisTime = millis() ; // Call this once only
+#ifdef ORANGE_TX
+ if ( USARTC0.STATUS & USART_RXCIF_bm )
+#elif defined STM32_BOARD
+ if ( USART2_BASE->SR & USART_SR_RXNE )
+#else
+ if ( UCSR0A & ( 1 << RXC0 ) )
+#endif
+ {
+ rxchar = UDR0 ;
+ BootCount += 1 ;
+ if ( ( lState == BOOT_WAIT_30_IDLE ) || ( lState == BOOT_WAIT_30_DATA ) )
+ {
+ if ( lState == BOOT_WAIT_30_IDLE ) // Waiting for 0x30
+ {
+ BootTimer = millisTime ; // Start timeout
+ }
+ if ( rxchar == 0x30 )
+ {
+ lState = BOOT_WAIT_20 ;
+ }
+ else
+ {
+ lState = BOOT_WAIT_30_DATA ;
+ }
+ }
+ else if ( lState == BOOT_WAIT_20 ) // Waiting for 0x20
+ {
+ if ( rxchar == 0x20 )
+ {
+ lState = BOOT_READY ;
+ }
+ }
+ }
+ else // No byte received
+ {
+ if ( lState != BOOT_WAIT_30_IDLE ) // Something received
+ {
+ uint8_t time = millisTime - BootTimer ;
+ if ( time > 5 )
+ {
+#ifdef STM32_BOARD
+ if ( BootCount > 4 )
+#else
+ if ( BootCount > 2 )
+#endif
+ { // Run normally
+ NotBootChecking = 0xFF ;
+ Mprotocol_serial_init( 0 ) ;
+ }
+ else if ( lState == BOOT_READY )
+ {
+#ifdef STM32_BOARD
+#define SCS_BASE (0xE000E000) /*!< System Control Space Base Address */
+#define SCB_BASE (SCS_BASE + 0x0D00) /*!< System Control Block Base Address */
+#define SCB ((SCB_Type *) SCB_BASE) /*!< SCB configuration struct */
+#define __I volatile /*!< defines 'read only' permissions */
+#define __IO volatile /*!< defines 'read / write' permissions */
+typedef struct
+{
+ __I uint32_t CPUID; /*!< Offset: 0x00 CPU ID Base Register */
+ __IO uint32_t ICSR; /*!< Offset: 0x04 Interrupt Control State Register */
+ __IO uint32_t VTOR; /*!< Offset: 0x08 Vector Table Offset Register */
+ __IO uint32_t AIRCR; /*!< Offset: 0x0C Application Interrupt / Reset Control Register */
+ __IO uint32_t SCR; /*!< Offset: 0x10 System Control Register */
+ __IO uint32_t CCR; /*!< Offset: 0x14 Configuration Control Register */
+ __IO uint8_t SHP[12]; /*!< Offset: 0x18 System Handlers Priority Registers (4-7, 8-11, 12-15) */
+ __IO uint32_t SHCSR; /*!< Offset: 0x24 System Handler Control and State Register */
+ __IO uint32_t CFSR; /*!< Offset: 0x28 Configurable Fault Status Register */
+ __IO uint32_t HFSR; /*!< Offset: 0x2C Hard Fault Status Register */
+ __IO uint32_t DFSR; /*!< Offset: 0x30 Debug Fault Status Register */
+ __IO uint32_t MMFAR; /*!< Offset: 0x34 Mem Manage Address Register */
+ __IO uint32_t BFAR; /*!< Offset: 0x38 Bus Fault Address Register */
+ __IO uint32_t AFSR; /*!< Offset: 0x3C Auxiliary Fault Status Register */
+ __I uint32_t PFR[2]; /*!< Offset: 0x40 Processor Feature Register */
+ __I uint32_t DFR; /*!< Offset: 0x48 Debug Feature Register */
+ __I uint32_t ADR; /*!< Offset: 0x4C Auxiliary Feature Register */
+ __I uint32_t MMFR[4]; /*!< Offset: 0x50 Memory Model Feature Register */
+ __I uint32_t ISAR[5]; /*!< Offset: 0x60 ISA Feature Register */
+} SCB_Type;
+#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */
+#define SCB_AIRCR_PRIGROUP_Msk (7ul << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */
+#define SCB_AIRCR_SYSRESETREQ_Msk (1ul << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+// NVIC_SystemReset() ;
+//static __INLINE void NVIC_SystemReset(void)
+ {
+ SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |
+ (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
+ SCB_AIRCR_SYSRESETREQ_Msk) ; /* Keep priority group unchanged */
+ asm("dsb");
+ while(1) ; /* wait until reset */
+ }
+#else
+ cli(); // Disable global int due to RW of 16 bits registers
+ void (*p)() ;
+#ifndef ORANGE_TX
+ p = (void (*)())0x3F00 ; // Word address (0x7E00 byte)
+#else
+ p = (void (*)())0x4000 ; // Word address (0x8000 byte)
+#endif
+ (*p)() ;
+ // go to boot
+#endif
+ }
+ else
+ {
+ lState = BOOT_WAIT_30_IDLE ;
+ BootCount = 0 ;
+ }
+ }
+ }
+ }
+ BootState = lState ;
+}
+#endif
+
+// Main
+// Protocol scheduler
+void loop()
+{
+ uint16_t next_callback,diff=0xFFFF;
+
+ while(1)
+ {
+ if(remote_callback==0 || IS_WAIT_BIND_on || diff>2*200)
+ {
+ do
+ {
+ Update_All();
+ }
+ while(remote_callback==0 || IS_WAIT_BIND_on);
+ }
+ #ifndef STM32_BOARD
+ if( (TIFR1 & OCF1A_bm) != 0)
+ {
+ cli(); // Disable global int due to RW of 16 bits registers
+ OCR1A=TCNT1; // Callback should already have been called... Use "now" as new sync point.
+ sei(); // Enable global int
+ }
+ else
+ while((TIFR1 & OCF1A_bm) == 0); // Wait before callback
+ #else
+ if((TIMER2_BASE->SR & TIMER_SR_CC1IF)!=0)
+ {
+ cli();
+ OCR1A = TCNT1;
+ sei();
+ }
+ else
+ while((TIMER2_BASE->SR & TIMER_SR_CC1IF )==0); // Wait before callback
+ #endif
+ do
+ {
+ TX_MAIN_PAUSE_on;
+ tx_pause();
+ if(IS_INPUT_SIGNAL_on && remote_callback!=0)
+ next_callback=remote_callback();
+ else
+ next_callback=2000; // No PPM/serial signal check again in 2ms...
+ TX_MAIN_PAUSE_off;
+ tx_resume();
+ while(next_callback>4000)
+ { // start to wait here as much as we can...
+ next_callback-=2000; // We will wait below for 2ms
+ cli(); // Disable global int due to RW of 16 bits registers
+ OCR1A += 2000*2 ; // set compare A for callback
+ #ifndef STM32_BOARD
+ TIFR1=OCF1A_bm; // clear compare A=callback flag
+ #else
+ TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag
+ #endif
+ sei(); // enable global int
+ if(Update_All()) // Protocol changed?
+ {
+ next_callback=0; // Launch new protocol ASAP
+ break;
+ }
+ #ifndef STM32_BOARD
+ while((TIFR1 & OCF1A_bm) == 0); // wait 2ms...
+ #else
+ while((TIMER2_BASE->SR & TIMER_SR_CC1IF)==0);//2ms wait
+ #endif
+ }
+ // at this point we have a maximum of 4ms in next_callback
+ next_callback *= 2 ;
+ cli(); // Disable global int due to RW of 16 bits registers
+ OCR1A+= next_callback ; // set compare A for callback
+ #ifndef STM32_BOARD
+ TIFR1=OCF1A_bm; // clear compare A=callback flag
+ #else
+ TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag write zero
+ #endif
+ diff=OCR1A-TCNT1; // compare timer and comparator
+ sei(); // enable global int
+ }
+ while(diff&0x8000); // Callback did not took more than requested time for next callback
+ // so we can launch Update_All before next callback
+ }
+}
+
+uint8_t Update_All()
+{
+ #ifdef ENABLE_SERIAL
+#ifdef CHECK_FOR_BOOTLOADER
+ if ( (mode_select==MODE_SERIAL) && (NotBootChecking == 0) )
+ {
+ pollBoot() ;
+ }
+ else
+#endif
+ if(mode_select==MODE_SERIAL && IS_RX_FLAG_on) // Serial mode and something has been received
+ {
+ update_serial_data(); // Update protocol and data
+ update_channels_aux();
+ INPUT_SIGNAL_on; //valid signal received
+ last_signal=millis();
+ }
+ #endif //ENABLE_SERIAL
+ #ifdef ENABLE_PPM
+ if(mode_select!=MODE_SERIAL && IS_PPM_FLAG_on) // PPM mode and a full frame has been received
+ {
+ for(uint8_t i=0;iPPM_MAX_125) temp_ppm=PPM_MAX_125;
+ Servo_data[i]= temp_ppm ;
+ }
+ PPM_FLAG_off; // wait for next frame before update
+ update_channels_aux();
+ INPUT_SIGNAL_on; //valid signal received
+ last_signal=millis();
+ }
+ #endif //ENABLE_PPM
+ update_led_status();
+ #if defined(TELEMETRY)
+ #if ( !( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) ) )
+ if((protocol==MODE_FRSKYD) || (protocol==MODE_BAYANG) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_FRSKYX) || (protocol==MODE_DSM) )
+ #endif
+ TelemetryUpdate();
+ #endif
+ #ifdef ENABLE_BIND_CH
+ if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && Servo_data[BIND_CH-1]>PPM_MAX_COMMAND && Servo_data[THROTTLE]<(servo_min_100+25))
+ { // Autobind is on and BIND_CH went up and Throttle is low
+ CHANGE_PROTOCOL_FLAG_on; //reload protocol to rebind
+ BIND_CH_PREV_on;
+ }
+ if(IS_BIND_CH_PREV_on && Servo_data[BIND_CH-1]2)
+ bind_counter=2;
+ }
+ #endif //ENABLE_BIND_CH
+ if(IS_CHANGE_PROTOCOL_FLAG_on)
+ { // Protocol needs to be changed or relaunched for bind
+ protocol_init(); //init new protocol
+ return 1;
+ }
+ return 0;
+}
+
+// Update channels direction and Servo_AUX flags based on servo AUX positions
+static void update_channels_aux(void)
+{
+ //Reverse channels direction
+ #ifdef REVERSE_AILERON
+ Servo_data[AILERON]=servo_mid-Servo_data[AILERON];
+ #endif
+ #ifdef REVERSE_ELEVATOR
+ Servo_data[ELEVATOR]=servo_mid-Servo_data[ELEVATOR];
+ #endif
+ #ifdef REVERSE_THROTTLE
+ Servo_data[THROTTLE]=servo_mid-Servo_data[THROTTLE];
+ #endif
+ #ifdef REVERSE_RUDDER
+ Servo_data[RUDDER]=servo_mid-Servo_data[RUDDER];
+ #endif
+ //Calc AUX flags
+ Servo_AUX=0;
+ for(uint8_t i=0;i<8;i++)
+ if(Servo_data[AUX1+i]>PPM_SWITCH)
+ Servo_AUX|=1<70)
+ INPUT_SIGNAL_off; //no valid signal (PPM or Serial) received for 70ms
+ if(blinkCR1 &= ~ USART_CR1_TXEIE;
+ #else
+ UCSR0B &= ~_BV(UDRIE0);
+ #endif
+ #endif
+ #endif
+ #endif
+}
+
+inline void tx_resume()
+{
+ #ifdef TELEMETRY
+ // Resume telemetry by enabling transmitter interrupt
+ if(!IS_TX_PAUSE_on)
+ {
+ #ifdef ORANGE_TX
+ cli() ;
+ USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ;
+ sei() ;
+ #else
+ #ifndef BASH_SERIAL
+ #ifdef STM32_BOARD
+ USART3_BASE->CR1 |= USART_CR1_TXEIE;
+ #else
+ UCSR0B |= _BV(UDRIE0);
+ #endif
+ #else
+ resumeBashSerial();
+ #endif
+ #endif
+ }
+ #endif
+}
+
+#ifdef STM32_BOARD
+void start_timer2()
+{
+ // Pause the timer while we're configuring it
+ timer.pause();
+ TIMER2_BASE->PSC = 35; //36-1;for 72 MHZ /0.5sec/(35+1)
+ TIMER2_BASE->ARR = 0xFFFF; //count till max
+ timer.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE);
+ timer.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE);
+ // Refresh the timer's count, prescale, and overflow
+ timer.refresh();
+ timer.resume();
+}
+#endif
+
+// Protocol start
+static void protocol_init()
+{
+ static uint16_t next_callback;
+ if(IS_WAIT_BIND_off)
+ {
+ remote_callback = 0; // No protocol
+ next_callback=0; // Default is immediate call back
+ LED_off; // Led off during protocol init
+ modules_reset(); // Reset all modules
+
+ // reset telemetry
+ #ifdef TELEMETRY
+ tx_pause();
+ pass=0;
+ telemetry_link=0;
+ telemetry_lost=1;
+ #ifdef BASH_SERIAL
+ TIMSK0 = 0 ; // Stop all timer 0 interrupts
+ #ifdef INVERT_SERIAL
+ SERIAL_TX_off;
+ #else
+ SERIAL_TX_on;
+ #endif
+ SerialControl.tail=0;
+ SerialControl.head=0;
+ SerialControl.busy=0;
+ #else
+ tx_tail=0;
+ tx_head=0;
+ #endif
+ TX_RX_PAUSE_off;
+ TX_MAIN_PAUSE_off;
+ #endif
+
+ //Set global ID and rx_tx_addr
+ MProtocol_id = RX_num + MProtocol_id_master;
+ set_rx_tx_addr(MProtocol_id);
+
+ blink=millis();
+
+ if(IS_BIND_BUTTON_FLAG_on)
+ AUTOBIND_FLAG_on;
+ if(IS_AUTOBIND_FLAG_on)
+ BIND_IN_PROGRESS; // Indicates bind in progress for blinking bind led
+ else
+ BIND_DONE;
+
+ PE1_on; //NRF24L01 antenna RF3 by default
+ PE2_off; //NRF24L01 antenna RF3 by default
+
+ switch(protocol) // Init the requested protocol
+ {
+ #ifdef A7105_INSTALLED
+ #if defined(FLYSKY_A7105_INO)
+ case MODE_FLYSKY:
+ PE1_off; //antenna RF1
+ next_callback = initFlySky();
+ remote_callback = ReadFlySky;
+ break;
+ #endif
+ #if defined(AFHDS2A_A7105_INO)
+ case MODE_AFHDS2A:
+ PE1_off; //antenna RF1
+ next_callback = initAFHDS2A();
+ remote_callback = ReadAFHDS2A;
+ break;
+ #endif
+ #if defined(HUBSAN_A7105_INO)
+ case MODE_HUBSAN:
+ PE1_off; //antenna RF1
+ if(IS_BIND_BUTTON_FLAG_on) random_id(10,true); // Generate new ID if bind button is pressed.
+ next_callback = initHubsan();
+ remote_callback = ReadHubsan;
+ break;
+ #endif
+ #endif
+ #ifdef CC2500_INSTALLED
+ #if defined(FRSKYD_CC2500_INO)
+ case MODE_FRSKYD:
+ PE1_off; //antenna RF2
+ PE2_on;
+ next_callback = initFrSky_2way();
+ remote_callback = ReadFrSky_2way;
+ break;
+ #endif
+ #if defined(FRSKYV_CC2500_INO)
+ case MODE_FRSKYV:
+ PE1_off; //antenna RF2
+ PE2_on;
+ next_callback = initFRSKYV();
+ remote_callback = ReadFRSKYV;
+ break;
+ #endif
+ #if defined(FRSKYX_CC2500_INO)
+ case MODE_FRSKYX:
+ PE1_off; //antenna RF2
+ PE2_on;
+ next_callback = initFrSkyX();
+ remote_callback = ReadFrSkyX;
+ break;
+ #endif
+ #if defined(SFHSS_CC2500_INO)
+ case MODE_SFHSS:
+ PE1_off; //antenna RF2
+ PE2_on;
+ next_callback = initSFHSS();
+ remote_callback = ReadSFHSS;
+ break;
+ #endif
+ #endif
+ #ifdef CYRF6936_INSTALLED
+ #if defined(DSM_CYRF6936_INO)
+ case MODE_DSM:
+ PE2_on; //antenna RF4
+ next_callback = initDsm();
+ //Servo_data[2]=1500;//before binding
+ remote_callback = ReadDsm;
+ break;
+ #endif
+ #if defined(DEVO_CYRF6936_INO)
+ case MODE_DEVO:
+ #ifdef ENABLE_PPM
+ if(mode_select) //PPM mode
+ {
+ if(IS_BIND_BUTTON_FLAG_on)
+ {
+ eeprom_write_byte((EE_ADDR)(30+mode_select),0x00); // reset to autobind mode for the current model
+ option=0;
+ }
+ else
+ {
+ option=eeprom_read_byte((EE_ADDR)(30+mode_select)); // load previous mode: autobind or fixed id
+ if(option!=1) option=0; // if not fixed id mode then it should be autobind
+ }
+ }
+ #endif //ENABLE_PPM
+ PE2_on; //antenna RF4
+ next_callback = DevoInit();
+ remote_callback = devo_callback;
+ break;
+ #endif
+ #if defined(WK2x01_CYRF6936_INO)
+ case MODE_WK2x01:
+ #ifdef ENABLE_PPM
+ if(mode_select) //PPM mode
+ {
+ if(IS_BIND_BUTTON_FLAG_on)
+ {
+ eeprom_write_byte((EE_ADDR)(30+mode_select),0x00); // reset to autobind mode for the current model
+ option=0;
+ }
+ else
+ {
+ option=eeprom_read_byte((EE_ADDR)(30+mode_select)); // load previous mode: autobind or fixed id
+ if(option!=1) option=0; // if not fixed id mode then it should be autobind
+ }
+ }
+ #endif //ENABLE_PPM
+ PE2_on; //antenna RF4
+ next_callback = WK_setup();
+ remote_callback = WK_cb;
+ break;
+ #endif
+ #if defined(J6PRO_CYRF6936_INO)
+ case MODE_J6PRO:
+ PE2_on; //antenna RF4
+ next_callback = initJ6Pro();
+ remote_callback = ReadJ6Pro;
+ break;
+ #endif
+ #endif
+ #ifdef NRF24L01_INSTALLED
+ #if defined(HISKY_NRF24L01_INO)
+ case MODE_HISKY:
+ next_callback=initHiSky();
+ remote_callback = hisky_cb;
+ break;
+ #endif
+ #if defined(V2X2_NRF24L01_INO)
+ case MODE_V2X2:
+ next_callback = initV2x2();
+ remote_callback = ReadV2x2;
+ break;
+ #endif
+ #if defined(YD717_NRF24L01_INO)
+ case MODE_YD717:
+ next_callback=initYD717();
+ remote_callback = yd717_callback;
+ break;
+ #endif
+ #if defined(KN_NRF24L01_INO)
+ case MODE_KN:
+ next_callback = initKN();
+ remote_callback = kn_callback;
+ break;
+ #endif
+ #if defined(SYMAX_NRF24L01_INO)
+ case MODE_SYMAX:
+ next_callback = initSymax();
+ remote_callback = symax_callback;
+ break;
+ #endif
+ #if defined(SLT_NRF24L01_INO)
+ case MODE_SLT:
+ next_callback=initSLT();
+ remote_callback = SLT_callback;
+ break;
+ #endif
+ #if defined(CX10_NRF24L01_INO)
+ case MODE_Q2X2:
+ sub_protocol|=0x08; // Increase the number of sub_protocols for CX-10
+ case MODE_CX10:
+ next_callback=initCX10();
+ remote_callback = CX10_callback;
+ break;
+ #endif
+ #if defined(CG023_NRF24L01_INO)
+ case MODE_CG023:
+ next_callback=initCG023();
+ remote_callback = CG023_callback;
+ break;
+ #endif
+ #if defined(BAYANG_NRF24L01_INO)
+ case MODE_BAYANG:
+ next_callback=initBAYANG();
+ remote_callback = BAYANG_callback;
+ break;
+ #endif
+ #if defined(ESKY_NRF24L01_INO)
+ case MODE_ESKY:
+ next_callback=initESKY();
+ remote_callback = ESKY_callback;
+ break;
+ #endif
+ #if defined(MT99XX_NRF24L01_INO)
+ case MODE_MT99XX:
+ next_callback=initMT99XX();
+ remote_callback = MT99XX_callback;
+ break;
+ #endif
+ #if defined(MJXQ_NRF24L01_INO)
+ case MODE_MJXQ:
+ next_callback=initMJXQ();
+ remote_callback = MJXQ_callback;
+ break;
+ #endif
+ #if defined(SHENQI_NRF24L01_INO)
+ case MODE_SHENQI:
+ next_callback=initSHENQI();
+ remote_callback = SHENQI_callback;
+ break;
+ #endif
+ #if defined(FY326_NRF24L01_INO)
+ case MODE_FY326:
+ next_callback=initFY326();
+ remote_callback = FY326_callback;
+ break;
+ #endif
+ #if defined(FQ777_NRF24L01_INO)
+ case MODE_FQ777:
+ next_callback=initFQ777();
+ remote_callback = FQ777_callback;
+ break;
+ #endif
+ #if defined(ASSAN_NRF24L01_INO)
+ case MODE_ASSAN:
+ next_callback=initASSAN();
+ remote_callback = ASSAN_callback;
+ break;
+ #endif
+ #if defined(HONTAI_NRF24L01_INO)
+ case MODE_HONTAI:
+ next_callback=initHONTAI();
+ remote_callback = HONTAI_callback;
+ break;
+ #endif
+ #if defined(Q303_NRF24L01_INO)
+ case MODE_Q303:
+ next_callback=initQ303();
+ remote_callback = Q303_callback;
+ break;
+ #endif
+ #if defined(GW008_NRF24L01_INO)
+ case MODE_GW008:
+ next_callback=initGW008();
+ remote_callback = GW008_callback;
+ break;
+ #endif
+ #if defined(DM002_NRF24L01_INO)
+ case MODE_DM002:
+ next_callback=initDM002();
+ remote_callback = DM002_callback;
+ break;
+ #endif
+ #endif
+ }
+ }
+
+ #if defined(WAIT_FOR_BIND) && defined(ENABLE_BIND_CH)
+ if( IS_AUTOBIND_FLAG_on && ! ( IS_BIND_CH_PREV_on || IS_BIND_BUTTON_FLAG_on || (cur_protocol[1]&0x80)!=0 ) )
+ {
+ WAIT_BIND_on;
+ return;
+ }
+ #endif
+ WAIT_BIND_off;
+ CHANGE_PROTOCOL_FLAG_off;
+
+ if(next_callback>32000)
+ { // next_callback should not be more than 32767 so we will wait here...
+ uint16_t temp=(next_callback>>10)-2;
+ delayMilliseconds(temp);
+ next_callback-=temp<<10; // between 2-3ms left at this stage
+ }
+ cli(); // disable global int
+ OCR1A = TCNT1 + next_callback*2; // set compare A for callback
+ sei(); // enable global int
+ #ifndef STM32_BOARD
+ TIFR1 = OCF1A_bm ; // clear compare A flag
+ #else
+ TIMER2_BASE->SR &= ~TIMER_SR_CC1IF; //clear compare Flag write zero
+ #endif
+ BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change
+}
+
+void update_serial_data()
+{
+ RX_DONOTUPDTAE_on;
+ RX_FLAG_off; //data is being processed
+ if(rx_ok_buff[1]&0x20) //check range
+ RANGE_FLAG_on;
+ else
+ RANGE_FLAG_off;
+ if(rx_ok_buff[1]&0xC0) //check autobind(0x40) & bind(0x80) together
+ AUTOBIND_FLAG_on;
+ else
+ AUTOBIND_FLAG_off;
+ if(rx_ok_buff[2]&0x80) //if rx_ok_buff[2] ==1,power is low ,0-power high
+ POWER_FLAG_off; //power low
+ else
+ POWER_FLAG_on; //power high
+
+ option=rx_ok_buff[3];
+
+ 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
+ WAIT_BIND_off;
+ protocol=(rx_ok_buff[0]==0x55?0:32) + (rx_ok_buff[1]&0x1F); //protocol no (0-63) bits 4-6 of buff[1] and bit 0 of buf[0]
+ sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6
+ RX_num=rx_ok_buff[2]& 0x0F; // rx_num bits 0---3
+ }
+ else
+ if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set
+ CHANGE_PROTOCOL_FLAG_on; //restart protocol with bind
+ else
+ if( ((rx_ok_buff[1]&0x80)==0) && ((cur_protocol[1]&0x80)!=0) ) // Bind flag has been reset
+ {
+ #if defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) || defined(FRSKYV_CC2500_INO)
+ if(protocol==MODE_FRSKYD || protocol==MODE_FRSKYX || protocol==MODE_FRSKYV)
+ BIND_DONE;
+ else
+ #endif
+ if(bind_counter>2)
+ bind_counter=2;
+ }
+
+ //store current protocol values
+ for(uint8_t i=0;i<3;i++)
+ cur_protocol[i] = rx_ok_buff[i];
+
+ // decode channel values
+ volatile uint8_t *p=rx_ok_buff+3;
+ uint8_t dec=-3;
+ for(uint8_t i=0;i=8)
+ {
+ dec-=8;
+ p++;
+ }
+ p++;
+ Servo_data[i]=((((*((uint32_t *)p))>>dec)&0x7FF)*5)/8+860; //value range 860<->2140 -125%<->+125%
+ }
+ RX_DONOTUPDTAE_off;
+ #ifdef ORANGE_TX
+ cli();
+ #else
+ UCSR0B &= ~_BV(RXCIE0); // RX interrupt disable
+ #endif
+ if(IS_RX_MISSED_BUFF_on) // If the buffer is still valid
+ { memcpy((void*)rx_ok_buff,(const void*)rx_buff,RXBUFFER_SIZE);// Duplicate the buffer
+ RX_FLAG_on; // data to be processed next time...
+ RX_MISSED_BUFF_off;
+ }
+ #ifdef ORANGE_TX
+ sei();
+ #else
+ UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
+ #endif
+}
+
+void modules_reset()
+{
+ #ifdef CC2500_INSTALLED
+ CC2500_Reset();
+ #endif
+ #ifdef A7105_INSTALLED
+ A7105_Reset();
+ #endif
+ #ifdef CYRF6936_INSTALLED
+ CYRF_Reset();
+ #endif
+ #ifdef NRF24L01_INSTALLED
+ NRF24L01_Reset();
+ #endif
+
+ //Wait for every component to reset
+ delayMilliseconds(100);
+ prev_power=0xFD; // unused power value
+}
+
+#ifdef CHECK_FOR_BOOTLOADER
+void Mprotocol_serial_init( uint8_t boot )
+#else
+void Mprotocol_serial_init()
+#endif
+{
+ #ifdef ORANGE_TX
+ PORTC.OUTSET = 0x08 ;
+ PORTC.DIRSET = 0x08 ;
+
+ USARTC0.BAUDCTRLA = 19 ;
+ USARTC0.BAUDCTRLB = 0 ;
+
+ USARTC0.CTRLB = 0x18 ;
+ USARTC0.CTRLA = (USARTC0.CTRLA & 0xCC) | 0x11 ;
+ USARTC0.CTRLC = 0x2B ;
+ UDR0 ;
+ #ifdef INVERT_SERIAL
+ PORTC.PIN3CTRL |= 0x40 ;
+ #endif
+#ifdef CHECK_FOR_BOOTLOADER
+ if ( boot )
+ {
+ USARTC0.BAUDCTRLB = 0 ;
+ USARTC0.BAUDCTRLA = 33 ; // 57600
+ USARTC0.CTRLA = (USARTC0.CTRLA & 0xC0) ;
+ USARTC0.CTRLC = 0x03 ; // 8 bit, no parity, 1 stop
+ USARTC0.CTRLB = 0x18 ; // Enable Tx and Rx
+ PORTC.PIN3CTRL &= ~0x40 ;
+ }
+#endif // CHECK_FOR_BOOTLOADER
+ #elif defined STM32_BOARD
+#ifdef CHECK_FOR_BOOTLOADER
+ if ( boot )
+ {
+ usart2_begin(57600,SERIAL_8N1);
+ USART2_BASE->CR1 &= ~USART_CR1_RXNEIE ;
+ (void)UDR0 ;
+ }
+ else
+ {
+ usart2_begin(100000,SERIAL_8E2);
+ USART2_BASE->CR1 |= USART_CR1_PCE_BIT;
+ }
+#else
+ usart2_begin(100000,SERIAL_8E2);
+ USART2_BASE->CR1 |= USART_CR1_PCE_BIT;
+#endif // CHECK_FOR_BOOTLOADER
+ usart3_begin(100000,SERIAL_8E2);
+ USART3_BASE->CR1 &= ~ USART_CR1_RE;//disable
+ USART2_BASE->CR1 &= ~ USART_CR1_TE;//disable transmit
+ #else
+ //ATMEGA328p
+ #include
+ UBRR0H = UBRRH_VALUE;
+ UBRR0L = UBRRL_VALUE;
+ UCSR0A = 0 ; // Clear X2 bit
+ //Set frame format to 8 data bits, even parity, 2 stop bits
+ UCSR0C = _BV(UPM01)|_BV(USBS0)|_BV(UCSZ01)|_BV(UCSZ00);
+ while ( UCSR0A & (1 << RXC0) )//flush receive buffer
+ UDR0;
+ //enable reception and RC complete interrupt
+ UCSR0B = _BV(RXEN0)|_BV(RXCIE0);//rx enable and interrupt
+ #ifndef DEBUG_TX
+ #if defined(TELEMETRY)
+ initTXSerial( SPEED_100K ) ;
+ #endif //TELEMETRY
+ #endif //DEBUG_TX
+#ifdef CHECK_FOR_BOOTLOADER
+ if ( boot )
+ {
+ UBRR0H = 0 ;
+ UBRR0L = 33 ; // 57600
+ UCSR0C &= ~_BV(UPM01) ; // No parity
+ UCSR0B &= ~_BV(RXCIE0);// No rx interrupt
+ UCSR0A |= _BV(U2X0); //Double speed mode USART0
+ }
+#endif // CHECK_FOR_BOOTLOADER
+ #endif //ORANGE_TX
+}
+
+#if defined(TELEMETRY)
+void PPM_Telemetry_serial_init()
+{
+ if( (protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_BAYANG) )
+ initTXSerial( SPEED_9600 ) ;
+ if(protocol==MODE_FRSKYX)
+ initTXSerial( SPEED_57600 ) ;
+ if(protocol==MODE_DSM)
+ initTXSerial( SPEED_125K ) ;
+}
+#endif
+
+// Convert 32b id to rx_tx_addr
+static void set_rx_tx_addr(uint32_t id)
+{ // Used by almost all protocols
+ rx_tx_addr[0] = (id >> 24) & 0xFF;
+ rx_tx_addr[1] = (id >> 16) & 0xFF;
+ rx_tx_addr[2] = (id >> 8) & 0xFF;
+ rx_tx_addr[3] = (id >> 0) & 0xFF;
+ rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F);
+}
+
+#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
+static void random_init(void)
+{
+ cli(); // Temporarily turn off interrupts, until WDT configured
+ MCUSR = 0; // Use the MCU status register to reset flags for WDR, BOR, EXTR, and POWR
+ WDTCSR |= _BV(WDCE); // WDT control register, This sets the Watchdog Change Enable (WDCE) flag, which is needed to set the prescaler
+ WDTCSR = _BV(WDIE); // Watchdog interrupt enable (WDIE)
+ sei(); // Turn interupts on
+}
+
+static uint32_t random_value(void)
+{
+ while (!gWDT_entropy);
+ return gWDT_entropy;
+}
+#endif
+
+static uint32_t random_id(uint16_t address, uint8_t create_new)
+{
+ #ifndef FORCE_GLOBAL_ID
+ uint32_t id=0;
+
+ if(eeprom_read_byte((EE_ADDR)(address+10))==0xf0 && !create_new)
+ { // TXID exists in EEPROM
+ for(uint8_t i=4;i>0;i--)
+ {
+ id<<=8;
+ id|=eeprom_read_byte((EE_ADDR)address+i-1);
+ }
+ if(id!=0x2AD141A7) //ID with seed=0
+ return id;
+ }
+ // Generate a random ID
+ #if defined STM32_BOARD
+ #define STM32_UUID ((uint32_t *)0x1FFFF7E8)
+ if (!create_new)
+ id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2];
+ #else
+ id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
+ #endif
+ for(uint8_t i=0;i<4;i++)
+ {
+ eeprom_write_byte((EE_ADDR)address+i,id);
+ id>>=8;
+ }
+ eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom.
+ return id;
+ #else
+ (void)address;
+ (void)create_new;
+ return FORCE_GLOBAL_ID;
+ #endif
+}
+
+/**************************/
+/**************************/
+/** Interrupt routines **/
+/**************************/
+/**************************/
+
+//PPM
+#ifdef ENABLE_PPM
+ #ifdef ORANGE_TX
+ #if PPM_pin == 2
+ ISR(PORTD_INT0_vect)
+ #else
+ ISR(PORTD_INT1_vect)
+ #endif
+ #elif defined STM32_BOARD
+ void PPM_decode()
+ #else
+ #if PPM_pin == 2
+ ISR(INT0_vect, ISR_NOBLOCK)
+ #else
+ ISR(INT1_vect, ISR_NOBLOCK)
+ #endif
+ #endif
+ { // Interrupt on PPM pin
+ static int8_t chan=0,bad_frame=1;
+ static uint16_t Prev_TCNT1=0;
+ uint16_t Cur_TCNT1;
+
+ Cur_TCNT1 = TCNT1 - Prev_TCNT1 ; // Capture current Timer1 value
+ if(Cur_TCNT1<1000)
+ bad_frame=1; // bad frame
+ else
+ if(Cur_TCNT1>4840)
+ { //start of frame
+ if(chan>=MIN_PPM_CHANNELS)
+ PPM_FLAG_on; // good frame received if at least 4 channels have been seen
+ chan=0; // reset channel counter
+ bad_frame=0;
+ }
+ else
+ if(bad_frame==0) // need to wait for start of frame
+ { //servo values between 500us and 2420us will end up here
+ PPM_data[chan]= Cur_TCNT1>>1;;
+ if(chan++>=MAX_PPM_CHANNELS)
+ bad_frame=1; // don't accept any new channels
+ }
+ Prev_TCNT1+=Cur_TCNT1;
+ }
+#endif //ENABLE_PPM
+
+//Serial RX
+#ifdef ENABLE_SERIAL
+ #ifdef ORANGE_TX
+ ISR(USARTC0_RXC_vect)
+ #elif defined STM32_BOARD
+ void __irq_usart2()
+ #else
+ ISR(USART_RX_vect)
+ #endif
+ { // RX interrupt
+ static uint8_t idx=0;
+ #ifdef ORANGE_TX
+ if((USARTC0.STATUS & 0x1C)==0) // Check frame error, data overrun and parity error
+ #elif defined STM32_BOARD
+ if((USART2_BASE->SR & USART_SR_RXNE) && (USART2_BASE->SR &0x0F)==0)
+ #else
+ UCSR0B &= ~_BV(RXCIE0) ; // RX interrupt disable
+ sei() ;
+ if((UCSR0A&0x1C)==0) // Check frame error, data overrun and parity error
+ #endif
+ { // received byte is ok to process
+ if(idx==0||discard_frame==1)
+ { // Let's try to sync at this point
+ idx=0;discard_frame=0;
+ RX_MISSED_BUFF_off; // If rx_buff was good it's not anymore...
+ rx_buff[0]=UDR0;
+ if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
+ {
+ TX_RX_PAUSE_on;
+ tx_pause();
+ #if defined STM32_BOARD
+ uint16_t OCR1B;
+ OCR1B =TCNT1+(6500L);
+ timer.setCompare(TIMER_CH2,OCR1B);
+ timer.attachCompare2Interrupt(ISR_COMPB);
+ #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
+ #endif
+ idx++;
+ }
+ }
+ else
+ {
+ rx_buff[idx++]=UDR0; // Store received byte
+ if(idx>=RXBUFFER_SIZE)
+ { // A full frame has been received
+ if(!IS_RX_DONOTUPDTAE_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
+ RX_FLAG_on; // flag for main to process servo data
+ }
+ else
+ RX_MISSED_BUFF_on; // notify that rx_buff is good
+ discard_frame=1; // start again
+ }
+ }
+ }
+ else
+ {
+ idx=UDR0; // Dummy read
+ discard_frame=1; // Error encountered discard full frame...
+ }
+ if(discard_frame==1)
+ {
+ #ifdef STM32_BOARD
+ detachInterrupt(2); // Disable interrupt on ch2
+ #else
+ CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
+ #endif
+ TX_RX_PAUSE_off;
+ tx_resume();
+ }
+ #if not defined (ORANGE_TX) && not defined (STM32_BOARD)
+ cli() ;
+ UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
+ #endif
+ }
+
+ //Serial timer
+ #ifdef ORANGE_TX
+ ISR(TCC1_CCB_vect)
+ #elif defined STM32_BOARD
+ void ISR_COMPB()
+ #else
+ ISR(TIMER1_COMPB_vect, ISR_NOBLOCK )
+ #endif
+ { // Timer1 compare B interrupt
+ discard_frame=1;
+ #ifdef STM32_BOARD
+ detachInterrupt(2); // Disable interrupt on ch2
+ #else
+ CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
+ #endif
+ tx_resume();
+ }
+#endif //ENABLE_SERIAL
+
+#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
+ // Random interrupt service routine called every time the WDT interrupt is triggered.
+ // It is only enabled at startup to generate a seed.
+ ISR(WDT_vect)
+ {
+ static uint8_t gWDT_buffer_position=0;
+ #define gWDT_buffer_SIZE 32
+ static uint8_t gWDT_buffer[gWDT_buffer_SIZE];
+ gWDT_buffer[gWDT_buffer_position] = TCNT1L; // Record the Timer 1 low byte (only one needed)
+ gWDT_buffer_position++; // every time the WDT interrupt is triggered
+ if (gWDT_buffer_position >= gWDT_buffer_SIZE)
+ {
+ // The following code is an implementation of Jenkin's one at a time hash
+ for(uint8_t gWDT_loop_counter = 0; gWDT_loop_counter < gWDT_buffer_SIZE; ++gWDT_loop_counter)
+ {
+ gWDT_entropy += gWDT_buffer[gWDT_loop_counter];
+ gWDT_entropy += (gWDT_entropy << 10);
+ gWDT_entropy ^= (gWDT_entropy >> 6);
+ }
+ gWDT_entropy += (gWDT_entropy << 3);
+ gWDT_entropy ^= (gWDT_entropy >> 11);
+ gWDT_entropy += (gWDT_entropy << 15);
+ WDTCSR = 0; // Disable Watchdog interrupt
+ }
+ }
+#endif
diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino
index d8b55b2..5b023ca 100644
--- a/Multiprotocol/Telemetry.ino
+++ b/Multiprotocol/Telemetry.ino
@@ -1,1074 +1,1225 @@
-/*
- 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 .
- */
-//**************************
-// Telemetry serial code *
-//**************************
-#if defined TELEMETRY
-
-uint8_t RetrySequence ;
-
-#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
- #define MULTI_TIME 500 //in ms
- uint32_t lastMulti = 0;
-#endif
-
-#if defined SPORT_TELEMETRY
- #define SPORT_TIME 12000 //12ms
- #define FRSKY_SPORT_PACKET_SIZE 8
- uint32_t last = 0;
- uint8_t sport_counter=0;
- uint8_t RxBt = 0;
- uint8_t sport = 0;
-
-struct t_fx_rx_packet
-{
- uint8_t validSequence ;
- uint8_t count ;
- uint8_t payload[6] ;
-} ;
-
-// Store for out of sequence packet
-struct t_fx_rx_packet FrskyxRxTelemetry ;
-
-
-#endif
-#if defined HUB_TELEMETRY
- #define USER_MAX_BYTES 6
- uint8_t prev_index;
-#endif
-
-#define START_STOP 0x7e
-#define BYTESTUFF 0x7d
-#define STUFF_MASK 0x20
-#define MAX_PKTX 10
-uint8_t pktx[MAX_PKTX];
-uint8_t pktx1[MAX_PKTX];
-uint8_t indx;
-uint8_t frame[18];
-
-#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
-static void multi_send_header(uint8_t type, uint8_t len)
-{
- Serial_write('M');
- #ifdef MULTI_TELEMETRY
- Serial_write('P');
- Serial_write(type);
- #else
- (void)type;
- #endif
- Serial_write(len);
-}
-
-static void multi_send_status()
-{
- multi_send_header(MULTI_TELEMETRY_STATUS, 5);
-
- // Build flags
- uint8_t flags=0;
- if (IS_INPUT_SIGNAL_on)
- flags |= 0x01;
- if (mode_select==MODE_SERIAL)
- flags |= 0x02;
- if (remote_callback != 0)
- {
- flags |= 0x04;
- if (IS_WAIT_BIND_on)
- flags |= 0x10;
- else
- if (!IS_BIND_DONE_on)
- flags |= 0x08;
- }
- Serial_write(flags);
-
- // Version number example: 1.1.6.1
- Serial_write(VERSION_MAJOR);
- Serial_write(VERSION_MINOR);
- Serial_write(VERSION_REVISION);
- Serial_write(VERSION_PATCH_LEVEL);
-}
-#endif
-
-#ifdef DSM_TELEMETRY
- #ifdef MULTI_TELEMETRY
- void DSM_frame()
- {
- if (pkt[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]);
-
- }
- 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]);
- }
- }
- #else
- void DSM_frame()
- {
- 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]);
- }
- #endif
-#endif
-
-#ifdef AFHDS2A_FW_TELEMETRY
- void AFHDSA_short_frame()
- {
- #if defined MULTI_TELEMETRY
- multi_send_header(MULTI_TELEMETRY_AFHDS2A, 29);
- #else
- Serial_write(0xAA); // Telemetry packet
- #endif
- for (uint8_t i = 0; i < 29; i++) // RSSI value followed by 4*7 bytes of telemetry data
- Serial_write(pkt[i]);
- }
-#endif
-
-#ifdef MULTI_TELEMETRY
-static void multi_send_frskyhub()
-{
- multi_send_header(MULTI_TELEMETRY_HUB, 9);
- for (uint8_t i = 0; i < 9; i++)
- Serial_write(frame[i]);
-}
-#endif
-
-void frskySendStuffed()
-{
- Serial_write(START_STOP);
- for (uint8_t i = 0; i < 9; i++)
- {
- if ((frame[i] == START_STOP) || (frame[i] == BYTESTUFF))
- {
- Serial_write(BYTESTUFF);
- frame[i] ^= STUFF_MASK;
- }
- Serial_write(frame[i]);
- }
- Serial_write(START_STOP);
-}
-
-void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
-{
- if(pkt[1] == rx_tx_addr[3] && pkt[2] == rx_tx_addr[2] && len ==(pkt[0] + 3))
- {
- telemetry_link|=1; // Telemetry data is available
- /*previous version
- RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>4);
- if(pktt[len-2] >=128) RSSI_dBm -= 164;
- else RSSI_dBm += 130;*/
- TX_RSSI = pkt[len-2];
- if(TX_RSSI >=128)
- TX_RSSI -= 128;
- else
- TX_RSSI += 128;
- TX_LQI = pkt[len-1]&0x7F;
- for (uint8_t i=3;i0 && pktt[6]<=10)
- {
- if (protocol==MODE_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
- }
- }
- }
- else
- {
- pktt[6]=0; // Discard packet
- }
- //
-#if defined SPORT_TELEMETRY && defined FRSKYX_CC2500_INO
- telemetry_lost=0;
- if (protocol==MODE_FRSKYX)
- {
- uint16_t lcrc = 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 ;
- }
- else if ( (pkt[5] & 0x03) == (FrX_receive_seq & 0x03 ) )
- {
- // OK to process
- FrX_receive_seq = ( FrX_receive_seq + 1 ) & 0x03 ;
- if ( FrskyxRxTelemetry.validSequence & 0x80 )
- {
- FrX_receive_seq = ( FrskyxRxTelemetry.validSequence + 1 ) & 3 ;
- }
- }
- else
- {
- // Save and request correct packet
- struct t_fx_rx_packet *p ;
- uint8_t count ;
- // pkt[4] RSSI
- // pkt[5] sequence control
- // pkt[6] payload count
- // pkt[7-12] payload
- pktt[6] = 0 ; // Don't process
- p = &FrskyxRxTelemetry ;
- count = pkt[6] ;
- if ( count <= 6 )
- {
- p->count = count ;
- for ( uint8_t i = 0 ; i < count ; i += 1 )
- {
- p->payload[i] = pkt[i+7] ;
- }
- p->validSequence = 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 ;
-// FrX_receive_seq = 0x08 ;
- }
- }
-
-// packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start
-// if ( FrX_send_seq != 0x08 )
-// {
-// FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ;
-// }
-
-// if ((pktt[5] >> 4 & 0x0f) == 0x08)
-// {
-// seq_last_sent = 8;
-// seq_last_rcvd = 0;
-// pass=0;
-// }
-// else
-// {
-// if ((pktt[5] >> 4 & 0x03) == (seq_last_rcvd + 1) % 4)
-// seq_last_rcvd = (seq_last_rcvd + 1) % 4;
-// else
-// pass=0;//reset if sequence wrong
-// }
- }
-#endif
- }
-}
-
-void init_frskyd_link_telemetry()
-{
- telemetry_link=0;
- telemetry_counter=0;
- v_lipo1=0;
- v_lipo2=0;
- RX_RSSI=0;
- TX_RSSI=0;
- RX_LQI=0;
- TX_LQI=0;
-}
-
-void frsky_link_frame()
-{
- frame[0] = 0xFE; // Link frame
- if (protocol==MODE_FRSKYD)
- {
- frame[1] = pktt[3]; // A1
- frame[2] = pktt[4]; // A2
- frame[3] = pktt[5]; // RX_RSSI
- telemetry_link &= ~1 ; // Sent
- telemetry_link |= 2 ; // Send hub if available
- }
- else
- if (protocol==MODE_HUBSAN||protocol==MODE_AFHDS2A||protocol==MODE_BAYANG)
- {
- frame[1] = v_lipo1;
- frame[2] = v_lipo2;
- frame[3] = RX_RSSI;
- telemetry_link=0;
- }
- frame[4] = TX_RSSI;
- frame[5] = RX_LQI;
- frame[6] = TX_LQI;
- frame[7] = frame[8] = 0;
- #if defined MULTI_TELEMETRY
- multi_send_frskyhub();
- #else
- frskySendStuffed();
- #endif
-}
-
-#if defined HUB_TELEMETRY
-void frsky_user_frame()
-{
- if(pktt[6])
- {//only send valid hub frames
- frame[0] = 0xFD; // user frame
- if(pktt[6]>USER_MAX_BYTES)
- {
- frame[1]=USER_MAX_BYTES; // packet size
- pktt[6]-=USER_MAX_BYTES;
- telemetry_link |= 2 ; // 2 packets need to be sent
- }
- else
- {
- frame[1]=pktt[6]; // packet size
- telemetry_link=0; // only 1 packet or processing second packet
- }
- frame[2] = pktt[7];
- for(uint8_t i=0;i0)
- {
- crc_s += p[i]; //0-1FF
- crc_s += crc_s >> 8; //0-100
- crc_s &= 0x00ff;
- }
- }
- }
-#else
- void sportSend(uint8_t *p)
- {
- uint16_t crc_s = 0;
- Serial_write(START_STOP);//+9
- Serial_write(p[0]) ;
- for (uint8_t i = 1; i < 9; i++)
- {
- if (i == 8)
- p[i] = 0xff - crc_s;
-
- if ((p[i] == START_STOP) || (p[i] == BYTESTUFF))
- {
- Serial_write(BYTESTUFF);//stuff again
- Serial_write(STUFF_MASK ^ p[i]);
- }
- else
- Serial_write(p[i]);
-
- if (i>0)
- {
- crc_s += p[i]; //0-1FF
- crc_s += crc_s >> 8; //0-100
- crc_s &= 0x00ff;
- }
- }
- }
-#endif
-
-void sportIdle()
-{
- #if !defined MULTI_TELEMETRY
- Serial_write(START_STOP);
- #endif
-}
-
-void sportSendFrame()
-{
- uint8_t i;
- sport_counter = (sport_counter + 1) %36;
- if(telemetry_lost)
- {
- sportIdle();
- return;
- }
- if(sport_counter<6)
- {
- frame[0] = 0x98;
- frame[1] = 0x10;
- for (i=5;i<8;i++)
- frame[i]=0;
- }
- switch (sport_counter)
- {
- case 0:
- frame[2] = 0x05;
- frame[3] = 0xf1;
- frame[4] = 0x02 ;//dummy values if swr 20230f00
- frame[5] = 0x23;
- frame[6] = 0x0F;
- break;
- case 2: // RSSI
- frame[2] = 0x01;
- frame[3] = 0xf1;
- frame[4] = RX_RSSI;
- frame[5] = TX_RSSI;
- frame[6] = RX_LQI;
- frame[7] = TX_LQI;
- break;
- case 4: //BATT
- frame[2] = 0x04;
- frame[3] = 0xf1;
- frame[4] = RxBt;//a1;
- break;
- default:
- if(sport)
- {
- for (i=0;i= FRSKY_SPORT_PACKET_SIZE)
- {//8 bytes no crc
- if ( sport )
- {
- // overrun!
- }
- else
- {
- uint8_t i ;
- for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i += 1 )
- {
- pktx1[i] = pktx[i] ; // Double buffer
- }
- sport = 1;//ok to send
- }
- pass = 0;//reset
- }
-}
-
-#endif
-
-void TelemetryUpdate()
-{
- // check for space in tx buffer
- #ifdef BASH_SERIAL
- uint8_t h ;
- uint8_t t ;
- h = SerialControl.head ;
- t = SerialControl.tail ;
- if ( h >= t )
- t += 128 - h ;
- else
- t -= h ;
- if ( t < 64 )
- return ;
- #else
- uint8_t h ;
- uint8_t t ;
- h = tx_head ;
- t = tx_tail ;
- if ( h >= t )
- t += TXBUFFER_SIZE - h ;
- else
- t -= h ;
- if ( t < 32 )
- return ;
- #endif
- #if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
- {
- uint32_t now = millis();
- if ((now - lastMulti) > MULTI_TIME)
- {
- multi_send_status();
- lastMulti = now;
- return;
- }
- }
- #endif
-
- #if defined SPORT_TELEMETRY
- if (protocol==MODE_FRSKYX)
- { // FrSkyX
- if(telemetry_link)
- {
- if(pktt[4] & 0x80)
- RX_RSSI=pktt[4] & 0x7F ;
- else
- RxBt = (pktt[4]<<1) + 1 ;
- if(pktt[6] && pktt[6]<=6)
- {
- for (uint8_t i=0; i < pktt[6]; i++)
- proces_sport_data(pktt[7+i]);
- if ( FrskyxRxTelemetry.validSequence & 0x80 )
- {
- // Process out of sequence packet
- for (uint8_t i=0; i < FrskyxRxTelemetry.count ; i++)
- {
- proces_sport_data( FrskyxRxTelemetry.payload[i] ) ;
- }
-// FrX_receive_seq = ( FrskyxRxTelemetry.validSequence + 1 ) & 3 ;
- FrskyxRxTelemetry.validSequence = 0 ;
- }
- }
- telemetry_link=0;
- }
- uint32_t now = micros();
- if ((now - last) > SPORT_TIME)
- {
- sportSendFrame();
- #ifdef STM32_BOARD
- last=now;
- #else
- last += SPORT_TIME ;
- #endif
- }
- }
- #endif
-
- #if defined DSM_TELEMETRY
- if(telemetry_link && protocol == MODE_DSM)
- { // DSM
- DSM_frame();
- telemetry_link=0;
- return;
- }
- #endif
- #if defined AFHDS2A_FW_TELEMETRY
- if(telemetry_link == 2 && protocol == MODE_AFHDS2A)
- {
- AFHDSA_short_frame();
- telemetry_link=0;
- return;
- }
- #endif
-
- if((telemetry_link & 1 )&& protocol != MODE_FRSKYX)
- { // FrSkyD + Hubsan + AFHDS2A + Bayang
- frsky_link_frame();
- return;
- }
- #if defined HUB_TELEMETRY
- if((telemetry_link & 2) && protocol == MODE_FRSKYD)
- { // FrSkyD
- frsky_user_frame();
- return;
- }
- #endif
-}
-
-
-/**************************/
-/**************************/
-/** Serial TX routines **/
-/**************************/
-/**************************/
-
-#ifndef BASH_SERIAL
- // Routines for normal serial output
- void Serial_write(uint8_t data)
- {
- uint8_t nextHead ;
- nextHead = tx_head + 1 ;
- if ( nextHead >= TXBUFFER_SIZE )
- nextHead = 0 ;
- tx_buff[nextHead]=data;
- tx_head = nextHead ;
- tx_resume();
- }
-
- void initTXSerial( uint8_t speed)
- {
- #ifdef ENABLE_PPM
- if(speed==SPEED_9600)
- { // 9600
- #ifdef ORANGE_TX
- USARTC0.BAUDCTRLA = 207 ;
- USARTC0.BAUDCTRLB = 0 ;
- USARTC0.CTRLB = 0x18 ;
- USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
- USARTC0.CTRLC = 0x03 ;
- #else
- #ifdef STM32_BOARD
- usart3_begin(9600,SERIAL_8N1); //USART3
- USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
- #else
- UBRR0H = 0x00;
- UBRR0L = 0x67;
- UCSR0A = 0 ; // Clear X2 bit
- //Set frame format to 8 data bits, none, 1 stop bit
- UCSR0C = (1<CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
- #else
- UBRR0H = 0x00;
- UBRR0L = 0x22;
- UCSR0A = 0x02 ; // Set X2 bit
- //Set frame format to 8 data bits, none, 1 stop bit
- UCSR0C = (1<CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
- #else
- UBRR0H = 0x00;
- UBRR0L = 0x07;
- UCSR0A = 0x00 ; // Clear X2 bit
- //Set frame format to 8 data bits, none, 1 stop bit
- UCSR0C = (1<SR & USART_SR_TXE)
- {
- #endif
- if(tx_head!=tx_tail)
- {
- if(++tx_tail>=TXBUFFER_SIZE)//head
- tx_tail=0;
- #ifdef STM32_BOARD
- USART3_BASE->DR=tx_buff[tx_tail];//clears TXE bit
- #else
- UDR0=tx_buff[tx_tail];
- #endif
- }
- if (tx_tail == tx_head)
- tx_pause(); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
- #ifdef STM32_BOARD
- }
- #endif
- }
- #ifdef STM32_BOARD
- void usart2_begin(uint32_t baud,uint32_t config )
- {
- usart_init(USART2);
- usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config);
- usart_set_baud_rate(USART2, STM32_PCLK1, baud);//
- usart_enable(USART2);
- }
- void usart3_begin(uint32_t baud,uint32_t config )
- {
- usart_init(USART3);
- usart_config_gpios_async(USART3,GPIOB,PIN_MAP[PB11].gpio_bit,GPIOB,PIN_MAP[PB10].gpio_bit,config);
- usart_set_baud_rate(USART3, STM32_PCLK1, baud);
- usart_enable(USART3);
- }
- #endif
-#else //BASH_SERIAL
-// Routines for bit-bashed serial output
-
-// Speed is 0 for 100K and 1 for 9600
-void initTXSerial( uint8_t speed)
-{
- TIMSK0 = 0 ; // Stop all timer 0 interrupts
- #ifdef INVERT_SERIAL
- SERIAL_TX_off;
- #else
- SERIAL_TX_on;
- #endif
- UCSR0B &= ~(1<>= 7 ; // Top bit
- if ( SerialControl.speed == SPEED_100K )
- {
- #ifdef INVERT_SERIAL
- byteLo |= 0x02 ; // Parity bit
- #else
- byteLo |= 0xFC ; // Stop bits
- #endif
- // calc parity
- temp = byte ;
- temp >>= 4 ;
- temp = byte ^ temp ;
- temp1 = temp ;
- temp1 >>= 2 ;
- temp = temp ^ temp1 ;
- temp1 = temp ;
- temp1 <<= 1 ;
- temp ^= temp1 ;
- temp &= 0x02 ;
- #ifdef INVERT_SERIAL
- byteLo ^= temp ;
- #else
- byteLo |= temp ;
- #endif
- }
- else
- {
- byteLo |= 0xFE ; // Stop bit
- }
- byte <<= 1 ;
- #ifdef INVERT_SERIAL
- byte |= 1 ; // Start bit
- #endif
- uint8_t next = (SerialControl.head + 2) & 0x7f ;
- if ( next != SerialControl.tail )
- {
- SerialControl.data[SerialControl.head] = byte ;
- SerialControl.data[SerialControl.head+1] = byteLo ;
- SerialControl.head = next ;
- }
- if(!IS_TX_PAUSE_on)
- tx_resume();
-}
-
-void resumeBashSerial()
-{
- cli() ;
- if ( SerialControl.busy == 0 )
- {
- sei() ;
- // Start the transmission here
- #ifdef INVERT_SERIAL
- GPIOR2 = 0 ;
- #else
- GPIOR2 = 0x01 ;
- #endif
- if ( SerialControl.speed == SPEED_100K )
- {
- GPIOR1 = 1 ;
- OCR0B = TCNT0 + 40 ;
- OCR0A = OCR0B + 210 ;
- TIFR0 = (1<>= 1
- GPIOR0 = byte ;
- if ( --GPIOR1 == 0 )
- {
- TIMSK0 &= ~(1<>= 1
- GPIOR2 = byte ;
- if ( --GPIOR1 == 0 )
- {
- if ( IS_TX_PAUSE_on )
- {
- SerialControl.busy = 0 ;
- TIMSK0 &= ~(1<head != ptr->tail )
- {
- GPIOR0 = ptr->data[ptr->tail] ;
- GPIOR2 = ptr->data[ptr->tail+1] ;
- ptr->tail = ( ptr->tail + 2 ) & 0x7F ;
- GPIOR1 = 8 ;
- OCR0A = OCR0B + 40 ;
- OCR0B = OCR0A + 8 * 20 ;
- TIMSK0 |= (1< 2 )
- {
- byte = GPIOR0 ;
- }
- else
- {
- byte = GPIOR2 ;
- }
- if ( byte & 0x01 )
- SERIAL_TX_on;
- else
- SERIAL_TX_off;
- byte /= 2 ; // Generates shorter code than byte >>= 1
- if ( GPIOR1 > 2 )
- {
- GPIOR0 = byte ;
- }
- else
- {
- GPIOR2 = byte ;
- }
- if ( --GPIOR1 == 0 )
- {
- // prepare next byte
- volatile struct t_serial_bash *ptr = &SerialControl ;
- if ( ptr->head != ptr->tail )
- {
- GPIOR0 = ptr->data[ptr->tail] ;
- GPIOR2 = ptr->data[ptr->tail+1] ;
- ptr->tail = ( ptr->tail + 2 ) & 0x7F ;
- GPIOR1 = 10 ;
- }
- else
- {
- SerialControl.busy = 0 ;
- TIMSK0 &= ~(1<.
+ */
+//**************************
+// Telemetry serial code *
+//**************************
+#if defined TELEMETRY
+
+uint8_t RetrySequence ;
+
+#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
+ #define MULTI_TIME 500 //in ms
+ uint32_t lastMulti = 0;
+#endif
+
+#if defined SPORT_TELEMETRY
+ #define SPORT_TIME 12000 //12ms
+ #define FRSKY_SPORT_PACKET_SIZE 8
+ uint32_t last = 0;
+ uint8_t sport_counter=0;
+ uint8_t RxBt = 0;
+ uint8_t sport = 0;
+
+//struct t_fx_rx_packet
+//{
+// uint8_t validSequence ;
+// uint8_t count ;
+// uint8_t payload[6] ;
+//} ;
+
+// Store for out of sequence packet
+//struct t_fx_rx_packet FrskyxRxTelemetry ;
+
+uint8_t FrskyxRxTelemetryValidSequence ;
+
+struct t_fx_rx_frame
+{
+ uint8_t valid ;
+ uint8_t count ;
+ uint8_t payload[6] ;
+} ;
+
+// Store for FrskyX telemetry
+struct t_fx_rx_frame FrskyxRxFrames[4] ;
+uint8_t NextFxFrameToForward ;
+
+#endif
+#if defined HUB_TELEMETRY
+ #define USER_MAX_BYTES 6
+ uint8_t prev_index;
+#endif
+
+#define START_STOP 0x7e
+#define BYTESTUFF 0x7d
+#define STUFF_MASK 0x20
+#define MAX_PKTX 10
+#define FX_BUFFERS 4
+uint8_t pktx[MAX_PKTX];
+uint8_t pktx1[FRSKY_SPORT_PACKET_SIZE*FX_BUFFERS];
+uint8_t indx;
+uint8_t frame[18];
+
+#if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
+static void multi_send_header(uint8_t type, uint8_t len)
+{
+ Serial_write('M');
+ #ifdef MULTI_TELEMETRY
+ Serial_write('P');
+ Serial_write(type);
+ #else
+ (void)type;
+ #endif
+ Serial_write(len);
+}
+
+static void multi_send_status()
+{
+ multi_send_header(MULTI_TELEMETRY_STATUS, 5);
+
+ // Build flags
+ uint8_t flags=0;
+ if (IS_INPUT_SIGNAL_on)
+ flags |= 0x01;
+ if (mode_select==MODE_SERIAL)
+ flags |= 0x02;
+ if (remote_callback != 0)
+ {
+ flags |= 0x04;
+ if (IS_WAIT_BIND_on)
+ flags |= 0x10;
+ else
+ if (!IS_BIND_DONE_on)
+ flags |= 0x08;
+ }
+ Serial_write(flags);
+
+ // Version number example: 1.1.6.1
+ Serial_write(VERSION_MAJOR);
+ Serial_write(VERSION_MINOR);
+ Serial_write(VERSION_REVISION);
+ Serial_write(VERSION_PATCH_LEVEL);
+}
+#endif
+
+#ifdef DSM_TELEMETRY
+ #ifdef MULTI_TELEMETRY
+ void DSM_frame()
+ {
+ if (pkt[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]);
+
+ }
+ 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]);
+ }
+ }
+ #else
+ void DSM_frame()
+ {
+ 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]);
+ }
+ #endif
+#endif
+
+#ifdef AFHDS2A_FW_TELEMETRY
+ void AFHDSA_short_frame()
+ {
+ #if defined MULTI_TELEMETRY
+ multi_send_header(MULTI_TELEMETRY_AFHDS2A, 29);
+ #else
+ Serial_write(0xAA); // Telemetry packet
+ #endif
+ for (uint8_t i = 0; i < 29; i++) // RSSI value followed by 4*7 bytes of telemetry data
+ Serial_write(pkt[i]);
+ }
+#endif
+
+#ifdef MULTI_TELEMETRY
+static void multi_send_frskyhub()
+{
+ multi_send_header(MULTI_TELEMETRY_HUB, 9);
+ for (uint8_t i = 0; i < 9; i++)
+ Serial_write(frame[i]);
+}
+#endif
+
+void frskySendStuffed()
+{
+ Serial_write(START_STOP);
+ for (uint8_t i = 0; i < 9; i++)
+ {
+ if ((frame[i] == START_STOP) || (frame[i] == BYTESTUFF))
+ {
+ Serial_write(BYTESTUFF);
+ frame[i] ^= STUFF_MASK;
+ }
+ Serial_write(frame[i]);
+ }
+ Serial_write(START_STOP);
+}
+
+void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
+{
+ uint8_t clen = pkt[0] + 3 ;
+ if(pkt[1] == rx_tx_addr[3] && pkt[2] == rx_tx_addr[2] && len == clen )
+ {
+ telemetry_link|=1; // Telemetry data is available
+ /*previous version
+ RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>4);
+ if(pktt[len-2] >=128) RSSI_dBm -= 164;
+ else RSSI_dBm += 130;*/
+ TX_RSSI = pkt[len-2];
+ if(TX_RSSI >=128)
+ TX_RSSI -= 128;
+ else
+ TX_RSSI += 128;
+ TX_LQI = pkt[len-1]&0x7F;
+ for (uint8_t i=3;i0 && pktt[6]<=10)
+ {
+ if (protocol==MODE_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
+ }
+ }
+ }
+ else
+ {
+ pktt[6]=0; // Discard packet
+ }
+ //
+#if defined SPORT_TELEMETRY && defined FRSKYX_CC2500_INO
+ telemetry_lost=0;
+ if (protocol==MODE_FRSKYX)
+ {
+ uint16_t lcrc = crc_x(&pkt[3], len-7 ) ;
+// if ( ( sub_protocol & 2 ) == 0 )
+// {
+// if ( ( (lcrc >> 8) == pkt[len-4]) && ( (lcrc & 0x00FF ) == pkt[len-3]) )
+// {
+// lcrc = 0 ;
+// }
+// else
+// {
+// lcrc = 1 ;
+// }
+// }
+// if ( lcrc == 0 )
+
+ 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 ;
+
+
+
+ FrX_receive_seq = ( FrX_receive_seq + 1 ) & 0x03 ;
+
+ if ( FrskyxRxTelemetryValidSequence & 0x80 )
+ {
+ FrX_receive_seq = ( FrskyxRxTelemetryValidSequence + 1 ) & 3 ;
+ FrskyxRxTelemetryValidSequence &= 0x7F ;
+ }
+
+// if ( FrskyxRxTelemetry.validSequence & 0x80 )
+// {
+// FrX_receive_seq = ( FrskyxRxTelemetry.validSequence + 1 ) & 3 ;
+
+// FrskyxRxTelemetry.validSequence &= 0x7F ;
+
+// }
+ }
+ else
+ {
+ // Save and request correct packet
+// struct t_fx_rx_packet *p ;
+ 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 ) ;
+ }
+
+
+// p = &FrskyxRxTelemetry ;
+// count = pkt[6] ;
+// if ( count <= 6 )
+// {
+// p->count = count ;
+// for ( uint8_t i = 0 ; i < count ; i += 1 )
+// {
+// p->payload[i] = pkt[i+7] ;
+// }
+// p->validSequence = 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 ;
+// FrX_receive_seq = 0x08 ;
+ }
+ }
+
+// packet[21] = (FrX_receive_seq << 4) | FrX_send_seq ;//8 at start
+// if ( FrX_send_seq != 0x08 )
+// {
+// FrX_send_seq = ( FrX_send_seq + 1 ) & 0x03 ;
+// }
+
+// if ((pktt[5] >> 4 & 0x0f) == 0x08)
+// {
+// seq_last_sent = 8;
+// seq_last_rcvd = 0;
+// pass=0;
+// }
+// else
+// {
+// if ((pktt[5] >> 4 & 0x03) == (seq_last_rcvd + 1) % 4)
+// seq_last_rcvd = (seq_last_rcvd + 1) % 4;
+// else
+// pass=0;//reset if sequence wrong
+// }
+ }
+#endif
+ }
+}
+
+void init_frskyd_link_telemetry()
+{
+ telemetry_link=0;
+ telemetry_counter=0;
+ v_lipo1=0;
+ v_lipo2=0;
+ RX_RSSI=0;
+ TX_RSSI=0;
+ RX_LQI=0;
+ TX_LQI=0;
+}
+
+void frsky_link_frame()
+{
+ frame[0] = 0xFE; // Link frame
+ if (protocol==MODE_FRSKYD)
+ {
+ frame[1] = pktt[3]; // A1
+ frame[2] = pktt[4]; // A2
+ frame[3] = pktt[5]; // RX_RSSI
+ telemetry_link &= ~1 ; // Sent
+ telemetry_link |= 2 ; // Send hub if available
+ }
+ else
+ if (protocol==MODE_HUBSAN||protocol==MODE_AFHDS2A||protocol==MODE_BAYANG)
+ {
+ frame[1] = v_lipo1;
+ frame[2] = v_lipo2;
+ frame[3] = RX_RSSI;
+ telemetry_link=0;
+ }
+ frame[4] = TX_RSSI;
+ frame[5] = RX_LQI;
+ frame[6] = TX_LQI;
+ frame[7] = frame[8] = 0;
+ #if defined MULTI_TELEMETRY
+ multi_send_frskyhub();
+ #else
+ frskySendStuffed();
+ #endif
+}
+
+#if defined HUB_TELEMETRY
+void frsky_user_frame()
+{
+ if(pktt[6])
+ {//only send valid hub frames
+ frame[0] = 0xFD; // user frame
+ if(pktt[6]>USER_MAX_BYTES)
+ {
+ frame[1]=USER_MAX_BYTES; // packet size
+ pktt[6]-=USER_MAX_BYTES;
+ telemetry_link |= 2 ; // 2 packets need to be sent
+ }
+ else
+ {
+ frame[1]=pktt[6]; // packet size
+ telemetry_link=0; // only 1 packet or processing second packet
+ }
+ frame[2] = pktt[7];
+ for(uint8_t i=0;i0)
+ {
+ crc_s += p[i]; //0-1FF
+ crc_s += crc_s >> 8; //0-100
+ crc_s &= 0x00ff;
+ }
+ }
+ }
+#else
+ void sportSend(uint8_t *p)
+ {
+ uint16_t crc_s = 0;
+ Serial_write(START_STOP);//+9
+ Serial_write(p[0]) ;
+ for (uint8_t i = 1; i < 9; i++)
+ {
+ if (i == 8)
+ p[i] = 0xff - crc_s;
+
+ if ((p[i] == START_STOP) || (p[i] == BYTESTUFF))
+ {
+ Serial_write(BYTESTUFF);//stuff again
+ Serial_write(STUFF_MASK ^ p[i]);
+ }
+ else
+ Serial_write(p[i]);
+
+ if (i>0)
+ {
+ crc_s += p[i]; //0-1FF
+ crc_s += crc_s >> 8; //0-100
+ crc_s &= 0x00ff;
+ }
+ }
+ }
+#endif
+
+void sportIdle()
+{
+ #if !defined MULTI_TELEMETRY
+ Serial_write(START_STOP);
+ #endif
+}
+
+void sportSendFrame()
+{
+ uint8_t i;
+ sport_counter = (sport_counter + 1) %36;
+ if(telemetry_lost)
+ {
+ sportIdle();
+ return;
+ }
+ if(sport_counter<6)
+ {
+ frame[0] = 0x98;
+ frame[1] = 0x10;
+ for (i=5;i<8;i++)
+ frame[i]=0;
+ }
+ switch (sport_counter)
+ {
+ case 0:
+ frame[2] = 0x05;
+ frame[3] = 0xf1;
+ frame[4] = 0x02 ;//dummy values if swr 20230f00
+ frame[5] = 0x23;
+ frame[6] = 0x0F;
+ break;
+ case 2: // RSSI
+ frame[2] = 0x01;
+ frame[3] = 0xf1;
+ frame[4] = RX_RSSI;
+ frame[5] = TX_RSSI;
+ frame[6] = RX_LQI;
+ frame[7] = TX_LQI;
+ break;
+ case 4: //BATT
+ frame[2] = 0x04;
+ frame[3] = 0xf1;
+ frame[4] = RxBt;//a1;
+ break;
+ default:
+ if(sport)
+ {
+ for (i=0;i= FRSKY_SPORT_PACKET_SIZE)
+ {//8 bytes no crc
+ if ( sport < FX_BUFFERS )
+ {
+ uint8_t dest = sport * FRSKY_SPORT_PACKET_SIZE ;
+ uint8_t i ;
+ for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i += 1 )
+ {
+ pktx1[dest++] = pktx[i] ; // Triple buffer
+ }
+ sport += 1 ;//ok to send
+ }
+// else
+// {
+// // Overrun
+// }
+ pass = 0;//reset
+ }
+}
+
+#endif
+
+void TelemetryUpdate()
+{
+ // check for space in tx buffer
+ #ifdef BASH_SERIAL
+ uint8_t h ;
+ uint8_t t ;
+ h = SerialControl.head ;
+ t = SerialControl.tail ;
+ if ( h >= t )
+ t += 192 - h ;
+ else
+ t -= h ;
+// if ( t < 32 )
+ if ( t < 64 )
+ {
+ return ;
+ }
+ #else
+ uint8_t h ;
+ uint8_t t ;
+ h = tx_head ;
+ t = tx_tail ;
+ if ( h >= t )
+ t += TXBUFFER_SIZE - h ;
+ else
+ t -= h ;
+ if ( t < 32 )
+ {
+ return ;
+ }
+ #endif
+ #if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) )
+ {
+ uint32_t now = millis();
+ if ((now - lastMulti) > MULTI_TIME)
+ {
+ multi_send_status();
+ lastMulti = now;
+ return;
+ }
+ }
+ #endif
+
+ #if defined SPORT_TELEMETRY
+ if (protocol==MODE_FRSKYX)
+ { // FrSkyX
+// struct t_fx_rx_frame *p ;
+// uint8_t count ;
+
+ for(;;)
+ {
+ struct t_fx_rx_frame *p ;
+ uint8_t count ;
+ p = &FrskyxRxFrames[NextFxFrameToForward] ;
+ 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 ;
+ }
+ else
+ {
+ break ;
+ }
+ }
+
+// p = &FrskyxRxFrames[NextFxFrameToForward] ;
+// 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 ;
+// }
+
+ if(telemetry_link)
+ {
+ if(pktt[4] & 0x80)
+ RX_RSSI=pktt[4] & 0x7F ;
+ else
+ RxBt = (pktt[4]<<1) + 1 ;
+
+// if(pktt[6] && pktt[6]<=6)
+// {
+// for (uint8_t i=0; i < pktt[6]; i++)
+// proces_sport_data(pktt[7+i]);
+// if ( FrskyxRxTelemetry.validSequence & 0x80 )
+// {
+// // Process out of sequence packet
+// for (uint8_t i=0; i < FrskyxRxTelemetry.count ; i++)
+// {
+// proces_sport_data( FrskyxRxTelemetry.payload[i] ) ;
+// }
+//// FrX_receive_seq = ( FrskyxRxTelemetry.validSequence + 1 ) & 3 ;
+// FrskyxRxTelemetry.validSequence = 0 ;
+// }
+// }
+
+
+ telemetry_link=0;
+ }
+ uint32_t now = micros();
+ if ((now - last) > SPORT_TIME)
+ {
+ sportSendFrame();
+ #ifdef STM32_BOARD
+ last=now;
+ #else
+ last += SPORT_TIME ;
+ #endif
+ }
+ }
+ #endif
+
+ #if defined DSM_TELEMETRY
+ if(telemetry_link && protocol == MODE_DSM)
+ { // DSM
+ DSM_frame();
+ telemetry_link=0;
+ return;
+ }
+ #endif
+ #if defined AFHDS2A_FW_TELEMETRY
+ if(telemetry_link == 2 && protocol == MODE_AFHDS2A)
+ {
+ AFHDSA_short_frame();
+ telemetry_link=0;
+ return;
+ }
+ #endif
+
+ if((telemetry_link & 1 )&& protocol != MODE_FRSKYX)
+ { // FrSkyD + Hubsan + AFHDS2A + Bayang
+ frsky_link_frame();
+ return;
+ }
+ #if defined HUB_TELEMETRY
+ if((telemetry_link & 2) && protocol == MODE_FRSKYD)
+ { // FrSkyD
+ frsky_user_frame();
+ return;
+ }
+ #endif
+}
+
+
+/**************************/
+/**************************/
+/** Serial TX routines **/
+/**************************/
+/**************************/
+
+#ifndef BASH_SERIAL
+ // Routines for normal serial output
+ void Serial_write(uint8_t data)
+ {
+ uint8_t nextHead ;
+ nextHead = tx_head + 1 ;
+ if ( nextHead >= TXBUFFER_SIZE )
+ nextHead = 0 ;
+ tx_buff[nextHead]=data;
+ tx_head = nextHead ;
+ tx_resume();
+ }
+
+ void initTXSerial( uint8_t speed)
+ {
+ #ifdef ENABLE_PPM
+ if(speed==SPEED_9600)
+ { // 9600
+ #ifdef ORANGE_TX
+ USARTC0.BAUDCTRLA = 207 ;
+ USARTC0.BAUDCTRLB = 0 ;
+ USARTC0.CTRLB = 0x18 ;
+ USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
+ USARTC0.CTRLC = 0x03 ;
+ #else
+ #ifdef STM32_BOARD
+ usart3_begin(9600,SERIAL_8N1); //USART3
+ USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
+ #else
+ UBRR0H = 0x00;
+ UBRR0L = 0x67;
+ UCSR0A = 0 ; // Clear X2 bit
+ //Set frame format to 8 data bits, none, 1 stop bit
+ UCSR0C = (1<CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
+ #else
+ UBRR0H = 0x00;
+ UBRR0L = 0x22;
+ UCSR0A = 0x02 ; // Set X2 bit
+ //Set frame format to 8 data bits, none, 1 stop bit
+ UCSR0C = (1<CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
+ #else
+ UBRR0H = 0x00;
+ UBRR0L = 0x07;
+ UCSR0A = 0x00 ; // Clear X2 bit
+ //Set frame format to 8 data bits, none, 1 stop bit
+ UCSR0C = (1<SR & USART_SR_TXE)
+ {
+ #endif
+ if(tx_head!=tx_tail)
+ {
+ if(++tx_tail>=TXBUFFER_SIZE)//head
+ tx_tail=0;
+ #ifdef STM32_BOARD
+ USART3_BASE->DR=tx_buff[tx_tail];//clears TXE bit
+ #else
+ UDR0=tx_buff[tx_tail];
+ #endif
+ }
+ if (tx_tail == tx_head)
+ tx_pause(); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
+ #ifdef STM32_BOARD
+ }
+ #endif
+ }
+ #ifdef STM32_BOARD
+ void usart2_begin(uint32_t baud,uint32_t config )
+ {
+ usart_init(USART2);
+ usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config);
+ usart_set_baud_rate(USART2, STM32_PCLK1, baud);//
+ usart_enable(USART2);
+ }
+ void usart3_begin(uint32_t baud,uint32_t config )
+ {
+ usart_init(USART3);
+ usart_config_gpios_async(USART3,GPIOB,PIN_MAP[PB11].gpio_bit,GPIOB,PIN_MAP[PB10].gpio_bit,config);
+ usart_set_baud_rate(USART3, STM32_PCLK1, baud);
+ usart_enable(USART3);
+ }
+ #endif
+#else //BASH_SERIAL
+// Routines for bit-bashed serial output
+
+// Speed is 0 for 100K and 1 for 9600
+void initTXSerial( uint8_t speed)
+{
+ TIMSK0 = 0 ; // Stop all timer 0 interrupts
+ #ifdef INVERT_SERIAL
+ SERIAL_TX_off;
+ #else
+ SERIAL_TX_on;
+ #endif
+ UCSR0B &= ~(1<>= 7 ; // Top bit
+ if ( SerialControl.speed == SPEED_100K )
+ {
+ #ifdef INVERT_SERIAL
+ byteLo |= 0x02 ; // Parity bit
+ #else
+ byteLo |= 0xFC ; // Stop bits
+ #endif
+ // calc parity
+ temp = byte ;
+ temp >>= 4 ;
+ temp = byte ^ temp ;
+ temp1 = temp ;
+ temp1 >>= 2 ;
+ temp = temp ^ temp1 ;
+ temp1 = temp ;
+ temp1 <<= 1 ;
+ temp ^= temp1 ;
+ temp &= 0x02 ;
+ #ifdef INVERT_SERIAL
+ byteLo ^= temp ;
+ #else
+ byteLo |= temp ;
+ #endif
+ }
+ else
+ {
+ byteLo |= 0xFE ; // Stop bit
+ }
+ byte <<= 1 ;
+ #ifdef INVERT_SERIAL
+ byte |= 1 ; // Start bit
+ #endif
+ uint8_t next = (SerialControl.head + 2) & 0x7f ;
+ if ( next != SerialControl.tail )
+ {
+ SerialControl.data[SerialControl.head] = byte ;
+ SerialControl.data[SerialControl.head+1] = byteLo ;
+ SerialControl.head = next ;
+ }
+ if(!IS_TX_PAUSE_on)
+ tx_resume();
+}
+
+void resumeBashSerial()
+{
+ cli() ;
+ if ( SerialControl.busy == 0 )
+ {
+ sei() ;
+ // Start the transmission here
+ #ifdef INVERT_SERIAL
+ GPIOR2 = 0 ;
+ #else
+ GPIOR2 = 0x01 ;
+ #endif
+ if ( SerialControl.speed == SPEED_100K )
+ {
+ GPIOR1 = 1 ;
+ OCR0B = TCNT0 + 40 ;
+ OCR0A = OCR0B + 210 ;
+ TIFR0 = (1<>= 1
+ GPIOR0 = byte ;
+ if ( --GPIOR1 == 0 )
+ {
+ TIMSK0 &= ~(1<>= 1
+ GPIOR2 = byte ;
+ if ( --GPIOR1 == 0 )
+ {
+ if ( IS_TX_PAUSE_on )
+ {
+ SerialControl.busy = 0 ;
+ TIMSK0 &= ~(1<head != ptr->tail )
+ {
+ GPIOR0 = ptr->data[ptr->tail] ;
+ GPIOR2 = ptr->data[ptr->tail+1] ;
+ uint8_t nextTail ;
+ nextTail = ptr->tail + 2 ;
+ if ( nextTail > 192 )
+ {
+ nextTail = 0 ;
+ }
+ ptr->tail = nextTail ;
+ GPIOR1 = 8 ;
+ OCR0A = OCR0B + 40 ;
+ OCR0B = OCR0A + 8 * 20 ;
+ TIMSK0 |= (1< 2 )
+ {
+ byte = GPIOR0 ;
+ }
+ else
+ {
+ byte = GPIOR2 ;
+ }
+ if ( byte & 0x01 )
+ SERIAL_TX_on;
+ else
+ SERIAL_TX_off;
+ byte /= 2 ; // Generates shorter code than byte >>= 1
+ if ( GPIOR1 > 2 )
+ {
+ GPIOR0 = byte ;
+ }
+ else
+ {
+ GPIOR2 = byte ;
+ }
+ if ( --GPIOR1 == 0 )
+ {
+ // prepare next byte
+ volatile struct t_serial_bash *ptr = &SerialControl ;
+ if ( ptr->head != ptr->tail )
+ {
+ GPIOR0 = ptr->data[ptr->tail] ;
+ GPIOR2 = ptr->data[ptr->tail+1] ;
+ ptr->tail = ( ptr->tail + 2 ) & 0x7F ;
+ GPIOR1 = 10 ;
+ }
+ else
+ {
+ SerialControl.busy = 0 ;
+ TIMSK0 &= ~(1<