| blob | fae9502cf9502e23de89c12d9403c218a837d737 |
1 /*********************************************************
2 Multiprotocol Tx code
3 by Midelic and Pascal Langer(hpnuts)
4 http://www.rcgroups.com/forums/showthread.php?t=2165676
5 https://github.com/pascallanger/DIY-Multiprotocol-TX-Module/edit/master/README.md
7 Thanks to PhracturedBlue, Hexfet, Goebish, Victzh and all protocol developers
8 Ported from deviation firmware
10 This project is free software: you can redistribute it and/or modify
11 it under the terms of the GNU General Public License as published by
12 the Free Software Foundation, either version 3 of the License, or
13 (at your option) any later version.
15 Multiprotocol is distributed in the hope that it will be useful,
16 but WITHOUT ANY WARRANTY; without even the implied warranty of
17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 GNU General Public License for more details.
20 You should have received a copy of the GNU General Public License
21 along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
22 */
23 #include <avr/pgmspace.h>
25 //#define DEBUG_PIN // Use pin TX for AVR and SPI_CS for STM32 => DEBUG_PIN_on, DEBUG_PIN_off, DEBUG_PIN_toggle
26 //#define DEBUG_SERIAL // Only for STM32_BOARD, compiled with Upload method "Serial"->usart1, "STM32duino bootloader"->USB serial
28 #ifdef __arm__ // Let's automatically select the board if arm is selected
29 #define STM32_BOARD
30 #endif
31 #if defined (ARDUINO_AVR_XMEGA32D4) || defined (ARDUINO_MULTI_ORANGERX)
32 #include "MultiOrange.h"
33 #endif
35 #include "Multiprotocol.h"
37 //Multiprotocol module configuration file
38 #include "_Config.h"
40 //Personal config file
41 #if defined(USE_MY_CONFIG)
42 #include "_MyConfig.h"
43 #endif
45 #include "Pins.h"
46 #include "TX_Def.h"
47 #include "Validate.h"
49 #ifndef STM32_BOARD
50 #include <avr/eeprom.h>
51 #else
52 #include <libmaple/usart.h>
53 #include <libmaple/timer.h>
54 //#include <libmaple/spi.h>
55 #include <SPI.h>
56 #include <EEPROM.h>
57 HardwareTimer HWTimer2(2);
58 #ifdef ENABLE_SERIAL
59 HardwareTimer HWTimer3(3);
60 void ISR_COMPB();
61 #endif
63 void PPM_decode();
64 extern "C"
65 {
66 void __irq_usart2(void);
67 void __irq_usart3(void);
68 }
69 #ifdef SEND_CPPM
70 HardwareTimer HWTimer1(1) ;
71 #endif
72 #endif
74 //Global constants/variables
75 uint32_t MProtocol_id;//tx id,
76 uint32_t MProtocol_id_master;
77 uint32_t blink=0,last_signal=0;
78 //
79 uint16_t counter;
80 uint8_t channel;
81 #if defined(ESKY150V2_CC2500_INO)
82 uint8_t packet[150];
83 #else
84 uint8_t packet[50];
85 #endif
87 #define NUM_CHN 16
88 // Servo data
89 uint16_t Channel_data[NUM_CHN];
90 uint8_t Channel_AUX;
91 #ifdef FAILSAFE_ENABLE
92 uint16_t Failsafe_data[NUM_CHN];
93 #endif
95 // Protocol variables
96 uint8_t cyrfmfg_id[6];//for dsm2 and devo
97 uint8_t rx_tx_addr[5];
98 uint8_t rx_id[5];
99 uint8_t phase;
100 uint16_t bind_counter;
101 uint8_t bind_phase;
102 uint8_t binding_idx;
103 uint16_t packet_period;
104 uint8_t packet_count;
105 uint8_t packet_sent;
106 uint8_t packet_length;
107 #if defined(HOTT_CC2500_INO) || defined(ESKY150V2_CC2500_INO) || defined(MLINK_CYRF6936_INO)
108 uint8_t hopping_frequency[78];
109 #else
110 uint8_t hopping_frequency[50];
111 #endif
112 uint8_t *hopping_frequency_ptr;
113 uint8_t hopping_frequency_no=0;
114 uint8_t rf_ch_num;
115 uint8_t throttle, rudder, elevator, aileron;
116 uint8_t flags;
117 uint16_t crc;
118 uint16_t crc16_polynomial;
119 uint8_t crc8;
120 uint8_t crc8_polynomial;
121 uint16_t seed;
122 uint16_t failsafe_count;
123 uint16_t state;
124 uint8_t len;
125 uint8_t armed, arm_flags, arm_channel_previous;
126 uint8_t num_ch;
127 uint32_t pps_timer;
128 uint16_t pps_counter;
130 #ifdef CC2500_INSTALLED
131 #ifdef SCANNER_CC2500_INO
132 uint8_t calData[255];
133 #elif defined(HOTT_CC2500_INO) || defined(ESKY150V2_CC2500_INO)
134 uint8_t calData[75];
135 #else
136 uint8_t calData[50];
137 #endif
138 #endif
140 #ifdef CHECK_FOR_BOOTLOADER
141 uint8_t BootTimer ;
142 uint8_t BootState ;
143 uint8_t NotBootChecking ;
144 uint8_t BootCount ;
146 #define BOOT_WAIT_30_IDLE 0
147 #define BOOT_WAIT_30_DATA 1
148 #define BOOT_WAIT_20 2
149 #define BOOT_READY 3
150 #endif
152 //Channel mapping for protocols
153 uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16};
154 uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16};
155 //uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16};
156 uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16};
158 // Mode_select variables
159 uint8_t mode_select;
160 uint8_t protocol_flags=0,protocol_flags2=0,protocol_flags3=0;
161 uint8_t option_override;
163 #ifdef ENABLE_PPM
164 // PPM variable
165 volatile uint16_t PPM_data[NUM_CHN];
166 volatile uint8_t PPM_chan_max=0;
167 uint32_t chan_order=0;
168 #endif
170 #if not defined (ORANGE_TX) && not defined (STM32_BOARD)
171 //Random variable
172 volatile uint32_t gWDT_entropy=0;
173 #endif
175 //Serial protocol
176 uint8_t sub_protocol;
177 uint8_t protocol;
178 uint8_t option;
179 uint8_t cur_protocol[3];
180 uint8_t prev_option;
181 uint8_t prev_power=0xFD; // unused power value
182 uint8_t RX_num;
184 //Serial RX variables
185 #define BAUD 100000
186 #define RXBUFFER_SIZE 36 // 26+1+9
187 volatile uint8_t rx_buff[RXBUFFER_SIZE];
188 volatile uint8_t rx_ok_buff[RXBUFFER_SIZE];
189 volatile bool discard_frame = false;
190 volatile uint8_t rx_idx=0, rx_len=0;
192 // Callback
193 uint16_function_t remote_callback = 0;
195 // Telemetry
196 #define TELEMETRY_BUFFER_SIZE 32
197 uint8_t packet_in[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets
198 #if defined(TELEMETRY)
199 #ifdef MULTI_SYNC
200 uint16_t last_serial_input=0;
201 uint16_t inputRefreshRate=0;
202 #endif
203 #ifdef INVERT_TELEMETRY
204 #if not defined(ORANGE_TX) && not defined(STM32_BOARD)
205 // enable bit bash for serial
206 #define BASH_SERIAL 1
207 #endif
208 #define INVERT_SERIAL 1
209 #endif
210 uint8_t telemetry_in_buffer[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets
211 #ifdef BASH_SERIAL
212 // For bit-bashed serial output
213 #define TXBUFFER_SIZE 192
214 volatile struct t_serial_bash
215 {
216 uint8_t head ;
217 uint8_t tail ;
218 uint8_t data[TXBUFFER_SIZE] ;
219 uint8_t busy ;
220 uint8_t speed ;
221 } SerialControl ;
222 #else
223 #define TXBUFFER_SIZE 96
224 volatile uint8_t tx_buff[TXBUFFER_SIZE];
225 volatile uint8_t tx_head=0;
226 volatile uint8_t tx_tail=0;
227 #endif // BASH_SERIAL
228 uint8_t v_lipo1;
229 uint8_t v_lipo2;
230 uint8_t RX_RSSI;
231 uint8_t TX_RSSI;
232 uint8_t RX_LQI;
233 uint8_t TX_LQI;
234 uint8_t telemetry_link=0;
235 uint8_t telemetry_counter=0;
236 uint8_t telemetry_lost;
237 #ifdef SPORT_SEND
238 #define MAX_SPORT_BUFFER 64
239 uint8_t SportData[MAX_SPORT_BUFFER];
240 uint8_t SportHead=0, SportTail=0;
241 #endif
243 // Functions definition when required
244 #ifdef HUB_TELEMETRY
245 static void __attribute__((unused)) frsky_send_user_frame(uint8_t, uint8_t, uint8_t);
246 #endif
248 //RX protocols
249 #if defined(AFHDS2A_RX_A7105_INO) || defined(FRSKY_RX_CC2500_INO) || defined(BAYANG_RX_NRF24L01_INO) || defined(DSM_RX_CYRF6936_INO)
250 bool rx_data_started;
251 bool rx_data_received;
252 bool rx_disable_lna;
253 uint16_t rx_rc_chan[16];
254 #endif
256 #ifdef HOTT_FW_TELEMETRY
257 uint8_t HoTT_SerialRX_val=0;
258 bool HoTT_SerialRX=false;
259 #endif
260 #ifdef DSM_FWD_PGM
261 uint8_t DSM_SerialRX_val[7];
262 bool DSM_SerialRX=false;
263 #endif
264 #ifdef MULTI_CONFIG_INO
265 uint8_t CONFIG_SerialRX_val[7];
266 bool CONFIG_SerialRX=false;
267 #endif
268 #endif // TELEMETRY
270 uint8_t multi_protocols_index=0xFF;
272 // Init
273 void setup()
274 {
275 // Setup diagnostic uart before anything else
276 #ifdef DEBUG_SERIAL
277 Serial.begin(115200,SERIAL_8N1);
279 // Wait up to 30s for a serial connection; double-blink the LED while we wait
280 unsigned long currMillis = millis();
281 unsigned long initMillis = currMillis;
282 pinMode(LED_pin,OUTPUT);
283 LED_off;
284 while (!Serial && (currMillis - initMillis) <= 30000) {
285 LED_on;
286 delay(100);
287 LED_off;
288 delay(100);
289 LED_on;
290 delay(100);
291 LED_off;
292 delay(500);
293 currMillis = millis();
294 }
296 delay(250); // Brief delay for FTDI debugging
297 debugln("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL);
298 #endif
300 // General pinout
301 #ifdef ORANGE_TX
302 //XMEGA
303 PORTD.OUTSET = 0x17 ;
304 PORTD.DIRSET = 0xB2 ;
305 PORTD.DIRCLR = 0x4D ;
306 PORTD.PIN0CTRL = 0x18 ;
307 PORTD.PIN2CTRL = 0x18 ;
308 PORTE.DIRSET = 0x01 ;
309 PORTE.DIRCLR = 0x02 ;
310 // Timer1 config
311 // TCC1 16-bit timer, clocked at 0.5uS
312 EVSYS.CH3MUX = 0x80 + 0x04 ; // Prescaler of 16
313 TCC1.CTRLB = 0; TCC1.CTRLC = 0; TCC1.CTRLD = 0; TCC1.CTRLE = 0;
314 TCC1.INTCTRLA = 0; TIMSK1 = 0;
315 TCC1.PER = 0xFFFF ;
316 TCNT1 = 0 ;
317 TCC1.CTRLA = 0x0B ; // Event3 (prescale of 16)
318 #elif defined STM32_BOARD
319 //STM32
320 afio_cfg_debug_ports(AFIO_DEBUG_NONE);
321 pinMode(LED_pin,OUTPUT);
322 pinMode(LED2_pin,OUTPUT);
323 pinMode(A7105_CSN_pin,OUTPUT);
324 pinMode(CC25_CSN_pin,OUTPUT);
325 pinMode(NRF_CSN_pin,OUTPUT);
326 pinMode(CYRF_CSN_pin,OUTPUT);
327 pinMode(SPI_CSN_pin,OUTPUT);
328 pinMode(CYRF_RST_pin,OUTPUT);
329 pinMode(PE1_pin,OUTPUT);
330 pinMode(PE2_pin,OUTPUT);
331 pinMode(TX_INV_pin,OUTPUT);
332 pinMode(RX_INV_pin,OUTPUT);
333 #if defined TELEMETRY
334 #if defined INVERT_SERIAL
335 TX_INV_on; // activate inverter for both serial TX and RX signals
336 RX_INV_on;
337 #else
338 TX_INV_off;
339 RX_INV_off;
340 #endif
341 #endif
342 pinMode(BIND_pin,INPUT_PULLUP);
343 pinMode(PPM_pin,INPUT);
344 pinMode(S1_pin,INPUT_PULLUP); // dial switch
345 pinMode(S2_pin,INPUT_PULLUP);
346 pinMode(S3_pin,INPUT_PULLUP);
347 pinMode(S4_pin,INPUT_PULLUP);
349 #ifdef MULTI_5IN1_INTERNAL
350 //pinMode(SX1276_RST_pin,OUTPUT); // already done by LED2_pin
351 pinMode(SX1276_TXEN_pin,OUTPUT); // PB0
352 pinMode(SX1276_DIO0_pin,INPUT_PULLUP);
353 #else
354 //Random pin
355 pinMode(RND_pin, INPUT_ANALOG); // set up PB0 pin for analog input
356 #endif
358 #if defined ENABLE_DIRECT_INPUTS
359 #if defined (DI1_PIN)
360 pinMode(DI1_PIN,INPUT_PULLUP);
361 #endif
362 #if defined (DI2_PIN)
363 pinMode(DI2_PIN,INPUT_PULLUP);
364 #endif
365 #if defined (DI3_PIN)
366 pinMode(DI3_PIN,INPUT_PULLUP);
367 #endif
368 #if defined (DI4_PIN)
369 pinMode(DI4_PIN,INPUT_PULLUP);
370 #endif
371 #endif
373 #ifdef SEND_CPPM
374 pinMode(PA9,INPUT); // make sure the USART1.TX pin is released for heartbeat use
375 #endif
377 //Timers
378 init_HWTimer(); //0.5us
380 //Read module flash size
381 #ifndef DISABLE_FLASH_SIZE_CHECK
382 unsigned short *flashSize = (unsigned short *) (0x1FFFF7E0);// Address register
383 debugln("Module Flash size: %dKB",(int)(*flashSize & 0xffff));
384 if((int)(*flashSize & 0xffff) < MCU_EXPECTED_FLASH_SIZE) // Not supported by this project
385 while (true) { //SOS
386 for(uint8_t i=0; i<3;i++)
387 {
388 LED_on;
389 delay(100);
390 LED_off;
391 delay(100);
392 }
393 for(uint8_t i=0; i<3;i++)
394 {
395 LED_on;
396 delay(500);
397 LED_off;
398 delay(100);
399 }
400 for(uint8_t i=0; i<3;i++)
401 {
402 LED_on;
403 delay(100);
404 LED_off;
405 delay(100);
406 }
407 LED_off;
408 delay(1000);
409 }
410 #endif
412 // Initialize the EEPROM
413 uint16_t eepromStatus = EEPROM.init();
414 debugln("EEPROM initialized: %d",eepromStatus);
416 // If there was no valid EEPROM page the EEPROM is corrupt or uninitialized and should be formatted
417 if( eepromStatus == EEPROM_NO_VALID_PAGE )
418 {
419 EEPROM.format();
420 debugln("No valid EEPROM page, EEPROM formatted");
421 }
422 #else
423 //ATMEGA328p
424 // all inputs
425 DDRB=0x00;DDRC=0x00;DDRD=0x00;
426 // outputs
427 SDI_output;
428 SCLK_output;
429 #ifdef A7105_CSN_pin
430 A7105_CSN_output;
431 #endif
432 #ifdef CC25_CSN_pin
433 CC25_CSN_output;
434 #endif
435 #ifdef CYRF_CSN_pin
436 CYRF_RST_output;
437 CYRF_CSN_output;
438 #endif
439 #ifdef NRF_CSN_pin
440 NRF_CSN_output;
441 #endif
442 PE1_output;
443 PE2_output;
444 SERIAL_TX_output;
446 // pullups
447 PROTO_DIAL1_port |= _BV(PROTO_DIAL1_pin);
448 PROTO_DIAL2_port |= _BV(PROTO_DIAL2_pin);
449 PROTO_DIAL3_port |= _BV(PROTO_DIAL3_pin);
450 PROTO_DIAL4_port |= _BV(PROTO_DIAL4_pin);
451 BIND_port |= _BV(BIND_pin);
453 // Timer1 config
454 TCCR1A = 0;
455 TCCR1B = (1 << CS11); //prescaler8, set timer1 to increment every 0.5us(16Mhz) and start timer
457 // Random
458 random_init();
459 #endif
461 LED2_on;
463 // Set Chip selects
464 #ifdef A7105_CSN_pin
465 A7105_CSN_on;
466 #endif
467 #ifdef CC25_CSN_pin
468 CC25_CSN_on;
469 #endif
470 #ifdef CYRF_CSN_pin
471 CYRF_CSN_on;
472 #endif
473 #ifdef NRF_CSN_pin
474 NRF_CSN_on;
475 #endif
476 #ifdef SPI_CSN_pin
477 SPI_CSN_on;
478 #endif
480 // Set SPI lines
481 #ifdef STM32_BOARD
482 initSPI2();
483 #else
484 SDI_on;
485 SCLK_off;
486 #endif
488 //Wait for every component to start
489 delayMilliseconds(100);
491 // Read status of bind button
492 if( IS_BIND_BUTTON_on )
493 {
494 BIND_BUTTON_FLAG_on; // If bind button pressed save the status
495 BIND_IN_PROGRESS; // Request bind
496 }
497 else
498 BIND_DONE;
500 // Read status of mode select binary switch
501 // after this mode_select will be one of {0000, 0001, ..., 1111}
502 #ifndef ENABLE_PPM
503 mode_select = MODE_SERIAL ; // force serial mode
504 #elif defined STM32_BOARD
505 mode_select= 0x0F -(uint8_t)(((GPIOA->regs->IDR)>>4)&0x0F);
506 #else
507 mode_select =
508 ((PROTO_DIAL1_ipr & _BV(PROTO_DIAL1_pin)) ? 0 : 1) +
509 ((PROTO_DIAL2_ipr & _BV(PROTO_DIAL2_pin)) ? 0 : 2) +
510 ((PROTO_DIAL3_ipr & _BV(PROTO_DIAL3_pin)) ? 0 : 4) +
511 ((PROTO_DIAL4_ipr & _BV(PROTO_DIAL4_pin)) ? 0 : 8);
512 #endif
513 //mode_select=1;
514 debugln("Protocol selection switch reads as %d", mode_select);
516 #ifdef ENABLE_PPM
517 uint8_t bank=bank_switch();
518 #endif
520 // Set default channels' value
521 for(uint8_t i=0;i<NUM_CHN;i++)
522 Channel_data[i]=1024;
523 Channel_data[THROTTLE]=0; //0=-125%, 204=-100%
525 #ifdef ENABLE_PPM
526 // Set default PPMs' value
527 for(uint8_t i=0;i<NUM_CHN;i++)
528 PPM_data[i]=PPM_MAX_100+PPM_MIN_100;
529 PPM_data[THROTTLE]=PPM_MIN_100*2;
530 #endif
532 // Update LED
533 LED_off;
534 LED_output;
536 //Init RF modules
537 modules_reset();
539 #ifndef ORANGE_TX
540 #ifdef STM32_BOARD
541 uint32_t seed=0;
542 for(uint8_t i=0;i<4;i++)
543 #ifdef RND_pin
544 seed=(seed<<8) | (analogRead(RND_pin)& 0xFF);
545 #else
546 //TODO find something to randomize...
547 seed=(seed<<8);
548 #endif
549 randomSeed(seed);
550 #else
551 //Init the seed with a random value created from watchdog timer for all protocols requiring random values
552 randomSeed(random_value());
553 #endif
554 #endif
556 // Read or create protocol id
557 MProtocol_id_master=random_id(EEPROM_ID_OFFSET,false);
559 debugln("Module Id: %lx", MProtocol_id_master);
561 #ifdef ENABLE_PPM
562 //Protocol and interrupts initialization
563 if(mode_select != MODE_SERIAL)
564 { // PPM
565 #ifndef MY_PPM_PROT
566 const PPM_Parameters *PPM_prot_line=&PPM_prot[bank*14+mode_select-1];
567 #else
568 const PPM_Parameters *PPM_prot_line=&My_PPM_prot[bank*14+mode_select-1];
569 #endif
571 protocol = PPM_prot_line->protocol;
572 cur_protocol[1] = protocol;
573 sub_protocol = PPM_prot_line->sub_proto;
574 RX_num = PPM_prot_line->rx_num;
575 chan_order = PPM_prot_line->chan_order;
577 //Forced frequency tuning values for CC2500 protocols
578 #if defined(FORCE_FRSKYD_TUNING) && defined(FRSKYD_CC2500_INO)
579 if(protocol==PROTO_FRSKYD)
580 option = FORCE_FRSKYD_TUNING; // Use config-defined tuning value for FrSkyD
581 else
582 #endif
583 #if defined(FORCE_FRSKYL_TUNING) && defined(FRSKYL_CC2500_INO)
584 if(protocol==PROTO_FRSKYL)
585 option = FORCE_FRSKYL_TUNING; // Use config-defined tuning value for FrSkyL
586 else
587 #endif
588 #if defined(FORCE_FRSKYV_TUNING) && defined(FRSKYV_CC2500_INO)
589 if(protocol==PROTO_FRSKYV)
590 option = FORCE_FRSKYV_TUNING; // Use config-defined tuning value for FrSkyV
591 else
592 #endif
593 #if defined(FORCE_FRSKYX_TUNING) && defined(FRSKYX_CC2500_INO)
594 if(protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2)
595 option = FORCE_FRSKYX_TUNING; // Use config-defined tuning value for FrSkyX
596 else
597 #endif
598 #if defined(FORCE_FUTABA_TUNING) && defined(FUTABA_CC2500_INO)
599 if (protocol==PROTO_FUTABA)
600 option = FORCE_FUTABA_TUNING; // Use config-defined tuning value for SFHSS
601 else
602 #endif
603 #if defined(FORCE_CORONA_TUNING) && defined(CORONA_CC2500_INO)
604 if (protocol==PROTO_CORONA)
605 option = FORCE_CORONA_TUNING; // Use config-defined tuning value for CORONA
606 else
607 #endif
608 #if defined(FORCE_SKYARTEC_TUNING) && defined(SKYARTEC_CC2500_INO)
609 if (protocol==PROTO_SKYARTEC)
610 option = FORCE_SKYARTEC_TUNING; // Use config-defined tuning value for SKYARTEC
611 else
612 #endif
613 #if defined(FORCE_REDPINE_TUNING) && defined(REDPINE_CC2500_INO)
614 if (protocol==PROTO_REDPINE)
615 option = FORCE_REDPINE_TUNING; // Use config-defined tuning value for REDPINE
616 else
617 #endif
618 #if defined(FORCE_RADIOLINK_TUNING) && defined(RADIOLINK_CC2500_INO)
619 if (protocol==PROTO_RADIOLINK)
620 option = FORCE_RADIOLINK_TUNING; // Use config-defined tuning value for RADIOLINK
621 else
622 #endif
623 #if defined(FORCE_HITEC_TUNING) && defined(HITEC_CC2500_INO)
624 if (protocol==PROTO_HITEC)
625 option = FORCE_HITEC_TUNING; // Use config-defined tuning value for HITEC
626 else
627 #endif
628 #if defined(FORCE_HOTT_TUNING) && defined(HOTT_CC2500_INO)
629 if (protocol==PROTO_HOTT)
630 option = FORCE_HOTT_TUNING; // Use config-defined tuning value for HOTT
631 else
632 #endif
633 option = (uint8_t)PPM_prot_line->option; // Use radio-defined option value
635 if(PPM_prot_line->power) POWER_FLAG_on;
636 if(PPM_prot_line->autobind)
637 {
638 AUTOBIND_FLAG_on;
639 BIND_IN_PROGRESS; // Force a bind at protocol startup
640 }
642 protocol_init();
644 #ifndef STM32_BOARD
645 //Configure PPM interrupt
646 #if PPM_pin == 2
647 EICRA |= _BV(ISC01); // The rising edge of INT0 pin D2 generates an interrupt request
648 EIMSK |= _BV(INT0); // INT0 interrupt enable
649 #elif PPM_pin == 3
650 EICRA |= _BV(ISC11); // The rising edge of INT1 pin D3 generates an interrupt request
651 EIMSK |= _BV(INT1); // INT1 interrupt enable
652 #else
653 #error PPM pin can only be 2 or 3
654 #endif
655 #else
656 attachInterrupt(PPM_pin,PPM_decode,FALLING);
657 #endif
659 #if defined(TELEMETRY)
660 PPM_Telemetry_serial_init();// Configure serial for telemetry
661 #endif
662 }
663 else
664 #endif //ENABLE_PPM
665 { // Serial
666 #ifdef ENABLE_SERIAL
667 for(uint8_t i=0;i<3;i++)
668 cur_protocol[i]=0;
669 protocol=0;
670 #ifdef CHECK_FOR_BOOTLOADER
671 Mprotocol_serial_init(1); // Configure serial and enable RX interrupt
672 #else
673 Mprotocol_serial_init(); // Configure serial and enable RX interrupt
674 #endif
675 #endif //ENABLE_SERIAL
676 }
677 debugln("Init complete");
678 LED2_on;
679 }
681 // Main
682 // Protocol scheduler
683 void loop()
684 {
685 uint16_t next_callback, diff;
686 uint8_t count=0;
688 while(1)
689 {
690 while(remote_callback==0 || IS_WAIT_BIND_on || IS_INPUT_SIGNAL_off)
691 if(!Update_All())
692 {
693 cli(); // Disable global int due to RW of 16 bits registers
694 OCR1A=TCNT1; // Callback should already have been called... Use "now" as new sync point.
695 sei(); // Enable global int
696 }
697 TX_MAIN_PAUSE_on;
698 tx_pause();
699 next_callback=remote_callback()<<1;
700 TX_MAIN_PAUSE_off;
701 tx_resume();
702 cli(); // Disable global int due to RW of 16 bits registers
703 OCR1A+=next_callback; // Calc when next_callback should happen
704 #ifndef STM32_BOARD
705 TIFR1=OCF1A_bm; // Clear compare A=callback flag
706 #else
707 TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag
708 #endif
709 diff=OCR1A-TCNT1; // Calc the time difference
710 sei(); // Enable global int
711 if((diff&0x8000) && !(next_callback&0x8000))
712 { // Negative result=callback should already have been called...
713 debugln("Short CB:%d",next_callback);
714 }
715 else
716 {
717 if(IS_RX_FLAG_on || IS_PPM_FLAG_on)
718 { // Serial or PPM is waiting...
719 if(++count>10)
720 { //The protocol does not leave enough time for an update so forcing it
721 count=0;
722 debugln("Force update");
723 Update_All();
724 }
725 }
726 #ifndef STM32_BOARD
727 while((TIFR1 & OCF1A_bm) == 0)
728 #else
729 while((TIMER2_BASE->SR & TIMER_SR_CC1IF )==0)
730 #endif
731 {
732 if(diff>900*2)
733 { //If at least 1ms is available update values
734 if((diff&0x8000) && !(next_callback&0x8000))
735 {//Should never get here...
736 debugln("!!!BUG!!!");
737 break;
738 }
739 count=0;
740 Update_All();
741 #ifdef DEBUG_SERIAL
742 if(TIMER2_BASE->SR & TIMER_SR_CC1IF )
743 debugln("Long update");
744 #endif
745 if(remote_callback==0)
746 break;
747 cli(); // Disable global int due to RW of 16 bits registers
748 diff=OCR1A-TCNT1; // Calc the time difference
749 sei(); // Enable global int
750 }
751 }
752 }
753 }
754 }
756 void End_Bind()
757 {
758 //Request protocol to terminate bind
759 if(protocol==PROTO_FRSKYD || protocol==PROTO_FRSKYL || protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || protocol==PROTO_FRSKYV || protocol==PROTO_FRSKY_R9
760 || protocol==PROTO_DSM_RX || protocol==PROTO_AFHDS2A_RX || protocol==PROTO_FRSKY_RX || protocol==PROTO_BAYANG_RX
761 || protocol==PROTO_AFHDS2A || protocol==PROTO_BUGS || protocol==PROTO_BUGSMINI || protocol==PROTO_HOTT || protocol==PROTO_ASSAN)
762 BIND_DONE;
763 else
764 if(bind_counter>2)
765 bind_counter=2;
766 }
768 void Update_Telem()
769 {
770 #if defined(TELEMETRY)
771 #ifndef MULTI_TELEMETRY
772 if((protocol == PROTO_BAYANG_RX) || (protocol == PROTO_AFHDS2A_RX) || (protocol == PROTO_FRSKY_RX) || (protocol == PROTO_SCANNER) || (protocol==PROTO_FRSKYD) || (protocol==PROTO_BAYANG) || (protocol==PROTO_NCC1701) || (protocol==PROTO_BUGS) || (protocol==PROTO_BUGSMINI) || (protocol==PROTO_HUBSAN) || (protocol==PROTO_AFHDS2A) || (protocol==PROTO_FRSKYX) || (protocol==PROTO_FRSKYX2) || (protocol==PROTO_DSM) || (protocol==PROTO_CABELL) || (protocol==PROTO_HITEC) || (protocol==PROTO_HOTT) || (protocol==PROTO_PROPEL) || (protocol==PROTO_OMP) || (protocol==PROTO_DEVO) || (protocol==PROTO_DSM_RX) || (protocol==PROTO_FRSKY_R9) || (protocol==PROTO_RLINK) || (protocol==PROTO_WFLY2) || (protocol==PROTO_LOLI) || (protocol==PROTO_MLINK) || (protocol==PROTO_MT99XX))
773 #endif
774 if(IS_DISABLE_TELEM_off)
775 TelemetryUpdate();
776 #endif
777 }
779 bool Update_All()
780 {
781 #ifdef ENABLE_SERIAL
782 #ifdef CHECK_FOR_BOOTLOADER
783 if ( (mode_select==MODE_SERIAL) && (NotBootChecking == 0) )
784 pollBoot() ;
785 else
786 #endif
787 if(mode_select==MODE_SERIAL && IS_RX_FLAG_on) // Serial mode and something has been received
788 {
789 update_serial_data(); // Update protocol and data
790 update_channels_aux();
791 INPUT_SIGNAL_on; //valid signal received
792 last_signal=millis();
793 }
794 #endif //ENABLE_SERIAL
795 #ifdef ENABLE_PPM
796 if(mode_select!=MODE_SERIAL && IS_PPM_FLAG_on) // PPM mode and a full frame has been received
797 {
798 uint32_t chan_or=chan_order;
799 uint8_t ch;
800 uint8_t channelsCount = PPM_chan_max;
802 #ifdef ENABLE_DIRECT_INPUTS
803 #ifdef DI_CH1_read
804 PPM_data[channelsCount] = DI_CH1_read;
805 channelsCount++;
806 #endif
807 #ifdef DI_CH2_read
808 PPM_data[channelsCount] = DI_CH2_read;
809 channelsCount++;
810 #endif
811 #ifdef DI_CH3_read
812 PPM_data[channelsCount] = DI_CH3_read;
813 channelsCount++;
814 #endif
815 #ifdef DI_CH4_read
816 PPM_data[channelsCount] = DI_CH4_read;
817 channelsCount++;
818 #endif
819 #endif
821 for(uint8_t i=0;i<channelsCount;i++)
822 { // update servo data without interrupts to prevent bad read
823 uint16_t val;
824 cli(); // disable global int
825 val = PPM_data[i];
826 sei(); // enable global int
827 val=map16b(val,PPM_MIN_100*2,PPM_MAX_100*2,CHANNEL_MIN_100,CHANNEL_MAX_100);
828 if(val&0x8000) val=CHANNEL_MIN_125;
829 else if(val>CHANNEL_MAX_125) val=CHANNEL_MAX_125;
830 if(chan_or)
831 {
832 ch=chan_or>>28;
833 if(ch)
834 Channel_data[ch-1]=val;
835 else
836 Channel_data[i]=val;
837 chan_or<<=4;
838 }
839 else
840 Channel_data[i]=val;
841 }
842 PPM_FLAG_off; // wait for next frame before update
843 #ifdef FAILSAFE_ENABLE
844 PPM_failsafe();
845 #endif
846 update_channels_aux();
847 INPUT_SIGNAL_on; // valid signal received
848 last_signal=millis();
849 }
850 #endif //ENABLE_PPM
851 update_led_status();
853 #ifdef SEND_CPPM
854 if ( telemetry_link & 0x80 )
855 { // Protocol requests telemetry to be disabled
856 if( protocol == PROTO_FRSKY_RX || protocol == PROTO_AFHDS2A_RX || protocol == PROTO_BAYANG_RX || protocol == PROTO_DSM_RX )
857 { // RX protocol
858 if(RX_LQI == 0)
859 telemetry_link = 0x00; // restore normal telemetry on connection loss
860 else if(telemetry_link & 1)
861 { // New data available
862 Send_CCPM_USART1();
863 telemetry_link &= 0xFE; // update done
864 }
865 }
866 }
867 else
868 #endif
870 Update_Telem();
872 #ifdef ENABLE_BIND_CH
873 if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && Channel_data[BIND_CH-1]>CHANNEL_MAX_COMMAND)
874 { // Autobind is on and BIND_CH went up
875 CHANGE_PROTOCOL_FLAG_on; // reload protocol
876 BIND_IN_PROGRESS; // enable bind
877 BIND_CH_PREV_on;
878 }
879 if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_on && Channel_data[BIND_CH-1]<CHANNEL_MIN_COMMAND)
880 { // Autobind is on and BIND_CH went down
881 BIND_CH_PREV_off;
882 End_Bind();
883 }
884 #endif //ENABLE_BIND_CH
885 if(IS_CHANGE_PROTOCOL_FLAG_on)
886 { // Protocol needs to be changed or relaunched for bind
887 protocol_init(); // init new protocol
888 return true;
889 }
890 return false;
891 }
893 #if defined(FAILSAFE_ENABLE) && defined(ENABLE_PPM)
894 void PPM_failsafe()
895 {
896 static uint8_t counter=0;
898 if(IS_BIND_IN_PROGRESS || IS_FAILSAFE_VALUES_on) // bind is not finished yet or Failsafe already being sent
899 return;
900 BIND_SET_INPUT;
901 BIND_SET_PULLUP;
902 if(IS_BIND_BUTTON_on)
903 {// bind button pressed
904 counter++;
905 if(counter>227)
906 { //after 5s with PPM frames @22ms
907 counter=0;
908 for(uint8_t i=0;i<NUM_CHN;i++)
909 Failsafe_data[i]=Channel_data[i];
910 FAILSAFE_VALUES_on;
911 }
912 }
913 else
914 counter=0;
915 BIND_SET_OUTPUT;
916 }
917 #endif
919 // Update channels direction and Channel_AUX flags based on servo AUX positions
920 static void update_channels_aux(void)
921 {
922 //Reverse channels direction
923 #ifdef REVERSE_AILERON
924 reverse_channel(AILERON);
925 #endif
926 #ifdef REVERSE_ELEVATOR
927 reverse_channel(ELEVATOR);
928 #endif
929 #ifdef REVERSE_THROTTLE
930 reverse_channel(THROTTLE);
931 #endif
932 #ifdef REVERSE_RUDDER
933 reverse_channel(RUDDER);
934 #endif
936 //Calc AUX flags
937 Channel_AUX=0;
938 for(uint8_t i=0;i<8;i++)
939 if(Channel_data[CH5+i]>CHANNEL_SWITCH)
940 Channel_AUX|=1<<i;
941 }
943 // Update led status based on binding and serial
944 static void update_led_status(void)
945 {
946 if(IS_INPUT_SIGNAL_on)
947 if(millis()-last_signal>70)
948 {
949 INPUT_SIGNAL_off; //no valid signal (PPM or Serial) received for 70ms
950 debugln("No input signal");
951 }
952 if(blink<millis())
953 {
954 if(IS_INPUT_SIGNAL_off)
955 {
956 if(mode_select==MODE_SERIAL)
957 blink+=BLINK_SERIAL_TIME; //blink slowly if no valid serial input
958 else
959 blink+=BLINK_PPM_TIME; //blink more slowly if no valid PPM input
960 }
961 else
962 if(remote_callback == 0)
963 { // Invalid protocol
964 if(IS_LED_on) //flash to indicate invalid protocol
965 blink+=BLINK_BAD_PROTO_TIME_LOW;
966 else
967 blink+=BLINK_BAD_PROTO_TIME_HIGH;
968 }
969 else
970 {
971 if(IS_WAIT_BIND_on)
972 {
973 if(IS_LED_on) //flash to indicate WAIT_BIND
974 blink+=BLINK_WAIT_BIND_TIME_LOW;
975 else
976 blink+=BLINK_WAIT_BIND_TIME_HIGH;
977 }
978 else
979 {
980 if(IS_BIND_DONE)
981 LED_off; //bind completed force led on
982 blink+=BLINK_BIND_TIME; //blink fastly during binding
983 }
984 }
985 LED_toggle;
986 }
987 }
989 #ifdef ENABLE_PPM
990 uint8_t bank_switch(void)
991 {
992 uint8_t bank=eeprom_read_byte((EE_ADDR)EEPROM_BANK_OFFSET);
993 if(bank>=NBR_BANKS)
994 { // Wrong number of bank
995 eeprom_write_byte((EE_ADDR)EEPROM_BANK_OFFSET,0x00); // set bank to 0
996 bank=0;
997 }
998 debugln("Using bank %d", bank);
1000 phase=3;
1001 uint32_t check=millis();
1002 blink=millis();
1003 while(mode_select==15)
1004 { //loop here if the dial is on position 15 for user to select the bank
1005 if(blink<millis())
1006 {
1007 switch(phase & 0x03)
1008 { // Flash bank number of times
1009 case 0:
1010 LED_on;
1011 blink+=BLINK_BANK_TIME_HIGH;
1012 phase++;
1013 break;
1014 case 1:
1015 LED_off;
1016 blink+=BLINK_BANK_TIME_LOW;
1017 phase++;
1018 break;
1019 case 2:
1020 if( (phase>>2) >= bank)
1021 {
1022 phase=0;
1023 blink+=BLINK_BANK_REPEAT;
1024 }
1025 else
1026 phase+=2;
1027 break;
1028 case 3:
1029 LED_output;
1030 LED_off;
1031 blink+=BLINK_BANK_TIME_LOW;
1032 phase=0;
1033 break;
1034 }
1035 }
1036 if(check<millis())
1037 {
1038 //Test bind button: for AVR it's shared with the LED so some extra work is needed to check it...
1039 #ifndef STM32_BOARD
1040 bool led=IS_LED_on;
1041 BIND_SET_INPUT;
1042 BIND_SET_PULLUP;
1043 #endif
1044 bool test_bind=IS_BIND_BUTTON_on;
1045 #ifndef STM32_BOARD
1046 if(led)
1047 LED_on;
1048 else
1049 LED_off;
1050 LED_output;
1051 #endif
1052 if( test_bind )
1053 { // Increase bank
1054 LED_on;
1055 bank++;
1056 if(bank>=NBR_BANKS)
1057 bank=0;
1058 eeprom_write_byte((EE_ADDR)EEPROM_BANK_OFFSET,bank);
1059 debugln("Using bank %d", bank);
1060 phase=3;
1061 blink+=BLINK_BANK_REPEAT;
1062 check+=2*BLINK_BANK_REPEAT;
1063 }
1064 check+=1;
1065 }
1066 }
1067 return bank;
1068 }
1069 #endif
1071 inline void tx_pause()
1072 {
1073 #ifdef TELEMETRY
1074 // Pause telemetry by disabling transmitter interrupt
1075 #ifdef ORANGE_TX
1076 USARTC0.CTRLA &= ~0x03 ;
1077 #else
1078 #ifndef BASH_SERIAL
1079 #ifdef STM32_BOARD
1080 USART3_BASE->CR1 &= ~ USART_CR1_TXEIE;
1081 #else
1082 UCSR0B &= ~_BV(UDRIE0);
1083 #endif
1084 #endif
1085 #endif
1086 #endif
1087 }
1089 inline void tx_resume()
1090 {
1091 #ifdef TELEMETRY
1092 // Resume telemetry by enabling transmitter interrupt
1093 if(IS_TX_PAUSE_off)
1094 {
1095 #ifdef ORANGE_TX
1096 cli() ;
1097 USARTC0.CTRLA = (USARTC0.CTRLA & 0xFC) | 0x01 ;
1098 sei() ;
1099 #else
1100 #ifndef BASH_SERIAL
1101 #ifdef STM32_BOARD
1102 USART3_BASE->CR1 |= USART_CR1_TXEIE;
1103 #else
1104 UCSR0B |= _BV(UDRIE0);
1105 #endif
1106 #else
1107 resumeBashSerial();
1108 #endif
1109 #endif
1110 }
1111 #endif
1112 }
1114 void rf_switch(uint8_t comp)
1115 {
1116 PE1_off;
1117 PE2_off;
1118 switch(comp)
1119 {
1120 case SW_CC2500:
1121 PE2_on;
1122 break;
1123 case SW_CYRF:
1124 PE2_on;
1125 case SW_NRF:
1126 PE1_on;
1127 break;
1128 }
1129 }
1131 // Protocol start
1132 static void protocol_init()
1133 {
1134 if(IS_WAIT_BIND_off)
1135 {
1136 remote_callback = 0; // No protocol
1137 LED_off; // Led off during protocol init
1138 crc16_polynomial = 0x1021; // Default CRC crc16_polynomial
1139 crc8_polynomial = 0x31; // Default CRC crc8_polynomial
1140 prev_option = option;
1142 multi_protocols_index = 0xFF;
1143 // reset telemetry
1144 #ifdef TELEMETRY
1145 #ifdef MULTI_SYNC
1146 inputRefreshRate = 0; // Don't do it unless the protocol asks for it
1147 #endif
1148 tx_pause();
1149 init_frskyd_link_telemetry();
1150 pps_timer=millis();
1151 pps_counter=0;
1152 #ifdef BASH_SERIAL
1153 TIMSK0 = 0 ; // Stop all timer 0 interrupts
1154 #ifdef INVERT_SERIAL
1155 SERIAL_TX_off;
1156 #else
1157 SERIAL_TX_on;
1158 #endif
1159 SerialControl.tail=0;
1160 SerialControl.head=0;
1161 SerialControl.busy=0;
1162 #else
1163 tx_tail=0;
1164 tx_head=0;
1165 #endif
1166 TX_RX_PAUSE_off;
1167 TX_MAIN_PAUSE_off;
1168 tx_resume();
1169 #if defined(AFHDS2A_RX_A7105_INO) || defined(FRSKY_RX_CC2500_INO) || defined(BAYANG_RX_NRF24L01_INO) || defined(DSM_RX_CYRF6936_INO)
1170 for(uint8_t ch=0; ch<16; ch++)
1171 rx_rc_chan[ch] = 1024;
1172 #endif
1173 #endif
1174 binding_idx=0;
1176 //Stop CPPM if it was previously running
1177 #ifdef SEND_CPPM
1178 release_trainer_ppm();
1179 #endif
1181 //Set global ID and rx_tx_addr
1182 MProtocol_id = RX_num + MProtocol_id_master;
1183 set_rx_tx_addr(MProtocol_id);
1185 #ifdef FAILSAFE_ENABLE
1186 FAILSAFE_VALUES_off;
1187 #endif
1188 DATA_BUFFER_LOW_off;
1190 SUB_PROTO_INVALID;
1191 option_override = 0xFF;
1193 blink=millis();
1195 debugln("Protocol selected: %d, sub proto %d, rxnum %d, option %d", protocol, sub_protocol, RX_num, option);
1196 if(protocol)
1197 {
1198 //Reset all modules
1199 modules_reset();
1201 uint8_t index=0;
1202 //#if defined(FRSKYX_CC2500_INO) && defined(MULTI_EU)
1203 // if( ! ( (protocol == PROTO_FRSKYX || protocol == PROTO_FRSKYX2) && sub_protocol < 2 ) )
1204 //#endif
1205 while(multi_protocols[index].protocol != 0xFF)
1206 {
1207 if(multi_protocols[index].protocol==protocol)
1208 {
1209 //Save index
1210 multi_protocols_index = index;
1211 //Check sub protocol validity
1212 if( ((sub_protocol&0x07) == 0) || (sub_protocol&0x07) < multi_protocols[index].nbrSubProto )
1213 SUB_PROTO_VALID;
1214 if(IS_SUB_PROTO_VALID)
1215 {//Start the protocol
1216 //Set the RF switch
1217 rf_switch(multi_protocols[index].rfSwitch);
1218 //Init protocol
1219 multi_protocols[index].Init(); // Init could invalidate the sub proto in case it is not suuported
1220 if(IS_SUB_PROTO_VALID)
1221 remote_callback = multi_protocols[index].CallBack; //Save call back function address
1222 }
1223 #ifdef DEBUG_SERIAL
1224 debug("Proto=%s", multi_protocols[index].ProtoString);
1225 debug(", nbr_sub=%d, Sub=", multi_protocols[index].nbrSubProto);
1226 if(IS_SUB_PROTO_VALID)
1227 {
1228 uint8_t len=multi_protocols[index].SubProtoString[0];
1229 uint8_t offset=len*(sub_protocol&0x07)+1;
1230 for(uint8_t j=0;j<len;j++)
1231 debug("%c",multi_protocols[index].SubProtoString[j+offset]);
1232 }
1233 debug(", Opt=%d",multi_protocols[index].optionType);
1234 debug(", FS=%d",multi_protocols[index].failSafe);
1235 debug(", CHMap=%d",multi_protocols[index].chMap);
1236 debugln(", rfSw=%d",multi_protocols[index].rfSwitch);
1237 #endif
1238 break;
1239 }
1240 index++;
1241 }
1242 //Send a telemetry status right now
1243 SEND_MULTI_STATUS_on;
1244 Update_Telem();
1245 }
1246 #ifdef MULTI_TELEMETRY
1247 else
1248 {//protocol=PROTO_PROTOLIST=0
1249 remote_callback = PROTOLIST_callback;
1250 prev_option = option + 1;
1251 }
1252 #endif
1253 }
1255 #if defined(WAIT_FOR_BIND) && defined(ENABLE_BIND_CH)
1256 if( IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && (cur_protocol[1]&0x80)==0 && mode_select == MODE_SERIAL)
1257 { // Autobind is active but no bind requested by either BIND_CH or BIND. But do not wait if in PPM mode...
1258 WAIT_BIND_on;
1259 return;
1260 }
1261 #endif
1262 WAIT_BIND_off;
1263 CHANGE_PROTOCOL_FLAG_off;
1265 if(protocol)
1266 {
1267 //Wait 5ms after protocol init
1268 cli(); // disable global int
1269 OCR1A = TCNT1 + 5000*2; // set compare A for callback
1270 #ifndef STM32_BOARD
1271 TIFR1 = OCF1A_bm ; // clear compare A flag
1272 #else
1273 TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag
1274 #endif
1275 sei(); // enable global int
1276 BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change
1277 }
1278 }
1280 void update_serial_data()
1281 {
1282 static bool prev_ch_mapping=false;
1283 #if defined(TELEMETRY) && defined(INVERT_TELEMETRY_TX)
1284 #ifdef INVERT_TELEMETRY
1285 static bool prev_inv_telem=true;
1286 #else
1287 static bool prev_inv_telem=false;
1288 #endif
1289 #endif
1291 RX_DONOTUPDATE_on;
1292 RX_FLAG_off; //data is being processed
1294 #ifdef SAMSON // Extremely dangerous, do not enable this unless you know what you are doing...
1295 if( rx_ok_buff[0]==0x55 && (rx_ok_buff[1]&0x1F)==PROTO_FRSKYD && rx_ok_buff[2]==0x7F && rx_ok_buff[24]==217 && rx_ok_buff[25]==202 )
1296 {//proto==FRSKYD+sub==7+rx_num==7+CH15==73%+CH16==73%
1297 rx_ok_buff[1]=(rx_ok_buff[1]&0xE0) | PROTO_FLYSKY; // change the protocol to Flysky
1298 memcpy((void*)(rx_ok_buff+4),(void*)(rx_ok_buff+4+11),11); // reassign channels 9-16 to 1-8
1299 }
1300 #endif
1301 #ifdef BONI // Extremely dangerous, do not enable this!!! This is really for a special case...
1302 if(CH14_SW)
1303 rx_ok_buff[2]=(rx_ok_buff[2]&0xF0)|((rx_ok_buff[2]+1)&0x0F);
1304 #endif
1306 if(rx_ok_buff[1]&0x20) //check range
1307 RANGE_FLAG_on;
1308 else
1309 RANGE_FLAG_off;
1310 if(rx_ok_buff[1]&0x40) //check autobind
1311 AUTOBIND_FLAG_on;
1312 else
1313 AUTOBIND_FLAG_off;
1314 if(rx_ok_buff[2]&0x80) //if rx_ok_buff[2] ==1,power is low ,0-power high
1315 POWER_FLAG_off; //power low
1316 else
1317 POWER_FLAG_on; //power high
1319 //Forced frequency tuning values for CC2500 protocols
1320 #if defined(FORCE_FRSKYD_TUNING) && defined(FRSKYD_CC2500_INO)
1321 if(protocol==PROTO_FRSKYD)
1322 option=FORCE_FRSKYD_TUNING; // Use config-defined tuning value for FrSkyD
1323 else
1324 #endif
1325 #if defined(FORCE_FRSKYL_TUNING) && defined(FRSKYL_CC2500_INO)
1326 if(protocol==PROTO_FRSKYL)
1327 option=FORCE_FRSKYL_TUNING; // Use config-defined tuning value for FrSkyL
1328 else
1329 #endif
1330 #if defined(FORCE_FRSKYV_TUNING) && defined(FRSKYV_CC2500_INO)
1331 if(protocol==PROTO_FRSKYV)
1332 option=FORCE_FRSKYV_TUNING; // Use config-defined tuning value for FrSkyV
1333 else
1334 #endif
1335 #if defined(FORCE_FRSKYX_TUNING) && defined(FRSKYX_CC2500_INO)
1336 if(protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2)
1337 option=FORCE_FRSKYX_TUNING; // Use config-defined tuning value for FrSkyX
1338 else
1339 #endif
1340 #if defined(FORCE_FUTABA_TUNING) && defined(FUTABA_CC2500_INO)
1341 if (protocol==PROTO_FUTABA)
1342 option=FORCE_FUTABA_TUNING; // Use config-defined tuning value for SFHSS
1343 else
1344 #endif
1345 #if defined(FORCE_CORONA_TUNING) && defined(CORONA_CC2500_INO)
1346 if (protocol==PROTO_CORONA)
1347 option=FORCE_CORONA_TUNING; // Use config-defined tuning value for CORONA
1348 else
1349 #endif
1350 #if defined(FORCE_SKYARTEC_TUNING) && defined(SKYARTEC_CC2500_INO)
1351 if (protocol==PROTO_SKYARTEC)
1352 option=FORCE_SKYARTEC_TUNING; // Use config-defined tuning value for SKYARTEC
1353 else
1354 #endif
1355 #if defined(FORCE_REDPINE_TUNING) && defined(REDPINE_CC2500_INO)
1356 if (protocol==PROTO_REDPINE)
1357 option=FORCE_REDPINE_TUNING; // Use config-defined tuning value for REDPINE
1358 else
1359 #endif
1360 #if defined(FORCE_RADIOLINK_TUNING) && defined(RADIOLINK_CC2500_INO)
1361 if (protocol==PROTO_RADIOLINK)
1362 option = FORCE_RADIOLINK_TUNING; // Use config-defined tuning value for RADIOLINK
1363 else
1364 #endif
1365 #if defined(FORCE_HITEC_TUNING) && defined(HITEC_CC2500_INO)
1366 if (protocol==PROTO_HITEC)
1367 option=FORCE_HITEC_TUNING; // Use config-defined tuning value for HITEC
1368 else
1369 #endif
1370 #if defined(FORCE_HOTT_TUNING) && defined(HOTT_CC2500_INO)
1371 if (protocol==PROTO_HOTT)
1372 option=FORCE_HOTT_TUNING; // Use config-defined tuning value for HOTT
1373 else
1374 #endif
1375 option=rx_ok_buff[3]; // Use radio-defined option value
1377 #ifdef FAILSAFE_ENABLE
1378 bool failsafe=false;
1379 if(rx_ok_buff[0]&0x02)
1380 { // Packet contains failsafe instead of channels
1381 failsafe=true;
1382 rx_ok_buff[0]&=0xFD; // Remove the failsafe flag
1383 FAILSAFE_VALUES_on; // Failsafe data has been received
1384 debugln("Failsafe received");
1385 }
1386 #endif
1388 DISABLE_CH_MAP_off;
1389 DISABLE_TELEM_off;
1390 if(rx_len>26)
1391 {//Additional flag received at the end
1392 rx_ok_buff[0]=(rx_ok_buff[26]&0xF0) | (rx_ok_buff[0]&0x0F); // Additional protocol numbers and RX_Num available -> store them in rx_ok_buff[0]
1393 if(rx_ok_buff[26]&0x02)
1394 DISABLE_TELEM_on;
1395 if(rx_ok_buff[26]&0x01)
1396 DISABLE_CH_MAP_on;
1397 #if defined(TELEMETRY) && defined(INVERT_TELEMETRY_TX)
1398 if(((rx_ok_buff[26]&0x08)!=0) ^ prev_inv_telem)
1399 { //value changed
1400 if(rx_ok_buff[26]&0x08)
1401 { // Invert telemetry
1402 debugln("Invert telem %d,%d",rx_ok_buff[26]&0x01,prev_inv_telem);
1403 #if defined (ORANGE_TX)
1404 PORTC.PIN3CTRL |= 0x40 ;
1405 #elif defined (STM32_BOARD)
1406 TX_INV_on;
1407 RX_INV_on;
1408 #endif
1409 }
1410 else
1411 { // Normal telemetry
1412 debugln("Normal telem %d,%d",rx_ok_buff[26]&0x01,prev_inv_telem);
1413 #if defined (ORANGE_TX)
1414 PORTC.PIN3CTRL &= 0xBF ;
1415 #elif defined (STM32_BOARD)
1416 TX_INV_off;
1417 RX_INV_off;
1418 #endif
1419 }
1420 prev_inv_telem=rx_ok_buff[26]&0x08;
1421 }
1422 #endif
1423 }
1425 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) ) )
1426 { // New model has been selected
1427 CHANGE_PROTOCOL_FLAG_on; //change protocol
1428 WAIT_BIND_off;
1429 if((rx_ok_buff[1]&0x80)!=0 || IS_AUTOBIND_FLAG_on)
1430 BIND_IN_PROGRESS; //launch bind right away if in autobind mode or bind is set
1431 else
1432 BIND_DONE;
1433 protocol=rx_ok_buff[1]&0x1F; //protocol no (0-31)
1434 if(!(rx_ok_buff[0]&1))
1435 protocol+=32; //protocol no (0-63)
1436 if(rx_len>26)
1437 protocol|=rx_ok_buff[26]&0xC0; //protocol no (0-255)
1438 sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6
1439 RX_num=rx_ok_buff[2]& 0x0F; //rx_num no (0-15)
1440 if(rx_len>26)
1441 RX_num|=rx_ok_buff[26]&0x30; //rx_num no (0-63)
1442 }
1443 else
1444 if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set
1445 { // Restart protocol with bind
1446 CHANGE_PROTOCOL_FLAG_on;
1447 BIND_IN_PROGRESS;
1448 }
1449 else
1450 if( ((rx_ok_buff[1]&0x80)==0) && ((cur_protocol[1]&0x80)!=0) ) // Bind flag has been reset
1451 { // Request protocol to end bind
1452 End_Bind();
1453 }
1455 //store current protocol values
1456 for(uint8_t i=0;i<3;i++)
1457 cur_protocol[i] = rx_ok_buff[i];
1459 //disable channel mapping
1460 if(multi_protocols[multi_protocols_index].chMap == 0)
1461 DISABLE_CH_MAP_off; //not a protocol supporting ch map to be disabled
1463 if(prev_ch_mapping!=IS_DISABLE_CH_MAP_on)
1464 {
1465 prev_ch_mapping=IS_DISABLE_CH_MAP_on;
1466 if(IS_DISABLE_CH_MAP_on)
1467 {
1468 for(uint8_t i=0;i<4;i++)
1469 CH_AETR[i]=CH_TAER[i]=CH_EATR[i]=i;
1470 debugln("DISABLE_CH_MAP_on");
1471 }
1472 else
1473 {
1474 CH_AETR[0]=AILERON;CH_AETR[1]=ELEVATOR;CH_AETR[2]=THROTTLE;CH_AETR[3]=RUDDER;
1475 CH_TAER[0]=THROTTLE;CH_TAER[1]=AILERON;CH_TAER[2]=ELEVATOR;CH_TAER[3]=RUDDER;
1476 CH_EATR[0]=ELEVATOR;CH_EATR[1]=AILERON;CH_EATR[2]=THROTTLE;CH_EATR[3]=RUDDER;
1477 debugln("DISABLE_CH_MAP_off");
1478 }
1479 }
1481 // decode channel/failsafe values
1482 volatile uint8_t *p=rx_ok_buff+3;
1483 uint8_t dec=-3;
1484 for(uint8_t i=0;i<NUM_CHN;i++)
1485 {
1486 dec+=3;
1487 if(dec>=8)
1488 {
1489 dec-=8;
1490 p++;
1491 }
1492 p++;
1493 uint16_t temp=((*((uint32_t *)p))>>dec)&0x7FF;
1494 #ifdef FAILSAFE_ENABLE
1495 if(failsafe)
1496 Failsafe_data[i]=temp; //value range 0..2047, 0=no pulse, 2047=hold
1497 else
1498 #endif
1499 Channel_data[i]=temp; //value range 0..2047, 0=-125%, 2047=+125%
1500 }
1502 #ifdef HOTT_FW_TELEMETRY
1503 HoTT_SerialRX=false;
1504 #endif
1505 if(rx_len>27)
1506 { // Data available for the current protocol
1507 #if defined(FRSKYX_CC2500_INO) || defined(FRSKYR9_SX1276_INO)
1508 if((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || protocol==PROTO_FRSKY_R9) && rx_len==28)
1509 {//Protocol waiting for 1 byte during bind
1510 binding_idx=rx_ok_buff[27];
1511 }
1512 #endif
1513 #ifdef SPORT_SEND
1514 if((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || protocol==PROTO_FRSKY_R9) && rx_len==27+8)
1515 {//Protocol waiting for 8 bytes
1516 #define BYTE_STUFF 0x7D
1517 #define STUFF_MASK 0x20
1518 //debug("SPort_in: ");
1519 boolean sport_valid=false;
1520 for(uint8_t i=28;i<28+7;i++)
1521 if(rx_ok_buff[i]!=0) sport_valid=true; //Check that the payload is not full of 0
1522 if((rx_ok_buff[27]&0x1F) > 0x1B) //Check 1st byte validity
1523 sport_valid=false;
1524 if(sport_valid)
1525 {
1526 SportData[SportTail]=0x7E;
1527 SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
1528 SportData[SportTail]=rx_ok_buff[27]&0x1F;
1529 SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
1530 for(uint8_t i=28;i<28+7;i++)
1531 {
1532 if( (rx_ok_buff[i]==BYTE_STUFF) || (rx_ok_buff[i]==0x7E) )
1533 {//stuff
1534 SportData[SportTail]=BYTE_STUFF;
1535 SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
1536 SportData[SportTail]=rx_ok_buff[i]^STUFF_MASK;
1537 }
1538 else
1539 SportData[SportTail]=rx_ok_buff[i];
1540 //debug("%02X ",SportData[SportTail]);
1541 SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
1542 }
1543 uint8_t used = SportTail;
1544 if ( SportHead > SportTail )
1545 used += MAX_SPORT_BUFFER - SportHead ;
1546 else
1547 used -= SportHead ;
1548 if ( used >= MAX_SPORT_BUFFER-(MAX_SPORT_BUFFER>>2) )
1549 {
1550 DATA_BUFFER_LOW_on;
1551 //Send Multi Status ASAP to inform the TX
1552 SEND_MULTI_STATUS_on;
1553 Update_Telem();
1554 debugln("Low buf=%d,h=%d,t=%d",used,SportHead,SportTail);
1555 }
1556 }
1557 }
1558 #endif //SPORT_SEND
1559 #ifdef HOTT_FW_TELEMETRY
1560 if(protocol==PROTO_HOTT && rx_len==27+1)
1561 {//Protocol waiting for 1 byte
1562 HoTT_SerialRX_val=rx_ok_buff[27];
1563 HoTT_SerialRX=true;
1564 }
1565 #endif
1566 #ifdef DSM_FWD_PGM
1567 if(protocol==PROTO_DSM && rx_len==27+7)
1568 {//Protocol waiting for 7 bytes
1569 memcpy(DSM_SerialRX_val, (const void *)&rx_ok_buff[27],7);
1570 DSM_SerialRX=true;
1571 }
1572 #endif
1573 #ifdef MULTI_CONFIG_INO
1574 if(protocol==PROTO_CONFIG && rx_len==27+7)
1575 {//Protocol waiting for 7 bytes
1576 memcpy(CONFIG_SerialRX_val, (const void *)&rx_ok_buff[27],7);
1577 CONFIG_SerialRX=true;
1578 }
1579 #endif
1580 }
1582 RX_DONOTUPDATE_off;
1583 #ifdef ORANGE_TX
1584 cli();
1585 #else
1586 UCSR0B &= ~_BV(RXCIE0); // RX interrupt disable
1587 #endif
1588 if(IS_RX_MISSED_BUFF_on) // If the buffer is still valid
1589 {
1590 if(rx_idx>=26 && rx_idx<RXBUFFER_SIZE)
1591 {
1592 rx_len=rx_idx;
1593 memcpy((void*)rx_ok_buff,(const void*)rx_buff,rx_len);// Duplicate the buffer
1594 RX_FLAG_on; // Data to be processed next time...
1595 }
1596 RX_MISSED_BUFF_off;
1597 }
1598 #ifdef ORANGE_TX
1599 sei();
1600 #else
1601 UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
1602 #endif
1603 }
1605 void modules_reset()
1606 {
1607 #ifdef CC2500_INSTALLED
1608 CC2500_Reset();
1609 #endif
1610 #ifdef A7105_INSTALLED
1611 A7105_Reset();
1612 #endif
1613 #ifdef CYRF6936_INSTALLED
1614 CYRF_Reset();
1615 #endif
1616 #ifdef NRF24L01_INSTALLED
1617 NRF24L01_Reset();
1618 #endif
1619 #ifdef SX1276_INSTALLED
1620 SX1276_Reset();
1621 #endif
1623 //Wait for every component to reset
1624 delayMilliseconds(100);
1625 prev_power=0xFD; // unused power value
1626 }
1628 #ifdef CHECK_FOR_BOOTLOADER
1629 void Mprotocol_serial_init( uint8_t boot )
1630 #else
1631 void Mprotocol_serial_init()
1632 #endif
1633 {
1634 #ifdef ORANGE_TX
1635 PORTC.OUTSET = 0x08 ;
1636 PORTC.DIRSET = 0x08 ;
1638 USARTC0.BAUDCTRLA = 19 ;
1639 USARTC0.BAUDCTRLB = 0 ;
1641 USARTC0.CTRLB = 0x18 ;
1642 USARTC0.CTRLA = (USARTC0.CTRLA & 0xCC) | 0x11 ;
1643 USARTC0.CTRLC = 0x2B ;
1644 UDR0 ;
1645 #ifdef INVERT_SERIAL
1646 PORTC.PIN3CTRL |= 0x40 ;
1647 #endif
1648 #ifdef CHECK_FOR_BOOTLOADER
1649 if ( boot )
1650 {
1651 USARTC0.BAUDCTRLB = 0 ;
1652 USARTC0.BAUDCTRLA = 33 ; // 57600
1653 USARTC0.CTRLA = (USARTC0.CTRLA & 0xC0) ;
1654 USARTC0.CTRLC = 0x03 ; // 8 bit, no parity, 1 stop
1655 USARTC0.CTRLB = 0x18 ; // Enable Tx and Rx
1656 PORTC.PIN3CTRL &= ~0x40 ;
1657 }
1658 #endif // CHECK_FOR_BOOTLOADER
1659 #elif defined STM32_BOARD
1660 #ifdef CHECK_FOR_BOOTLOADER
1661 if ( boot )
1662 {
1663 usart2_begin(57600,SERIAL_8N1);
1664 USART2_BASE->CR1 &= ~USART_CR1_RXNEIE ;
1665 (void)UDR0 ;
1666 }
1667 else
1668 #endif // CHECK_FOR_BOOTLOADER
1669 {
1670 usart2_begin(100000,SERIAL_8E2);
1671 USART2_BASE->CR1 |= USART_CR1_PCE_BIT;
1672 }
1673 USART2_BASE->CR1 &= ~ USART_CR1_TE; //disable transmit
1674 usart3_begin(100000,SERIAL_8E2);
1675 #else
1676 //ATMEGA328p
1677 #include <util/setbaud.h>
1678 UBRR0H = UBRRH_VALUE;
1679 UBRR0L = UBRRL_VALUE;
1680 UCSR0A = 0 ; // Clear X2 bit
1681 //Set frame format to 8 data bits, even parity, 2 stop bits
1682 UCSR0C = _BV(UPM01)|_BV(USBS0)|_BV(UCSZ01)|_BV(UCSZ00);
1683 while ( UCSR0A & (1 << RXC0) ) //flush receive buffer
1684 UDR0;
1685 //enable reception and RC complete interrupt
1686 UCSR0B = _BV(RXEN0)|_BV(RXCIE0);//rx enable and interrupt
1687 #ifndef DEBUG_PIN
1688 #if defined(TELEMETRY)
1689 initTXSerial( SPEED_100K ) ;
1690 #endif //TELEMETRY
1691 #endif //DEBUG_PIN
1692 #ifdef CHECK_FOR_BOOTLOADER
1693 if ( boot )
1694 {
1695 UBRR0H = 0;
1696 UBRR0L = 33; // 57600
1697 UCSR0C &= ~_BV(UPM01); // No parity
1698 UCSR0B &= ~_BV(RXCIE0); // No rx interrupt
1699 UCSR0A |= _BV(U2X0); // Double speed mode USART0
1700 }
1701 #endif // CHECK_FOR_BOOTLOADER
1702 #endif //ORANGE_TX
1703 }
1705 #ifdef STM32_BOARD
1706 void usart2_begin(uint32_t baud,uint32_t config )
1707 {
1708 usart_init(USART2);
1709 usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config);
1710 LED2_output;
1711 usart_set_baud_rate(USART2, STM32_PCLK1, baud);
1712 usart_enable(USART2);
1713 }
1714 void usart3_begin(uint32_t baud,uint32_t config )
1715 {
1716 usart_init(USART3);
1717 usart_config_gpios_async(USART3,GPIOB,PIN_MAP[PB11].gpio_bit,GPIOB,PIN_MAP[PB10].gpio_bit,config);
1718 usart_set_baud_rate(USART3, STM32_PCLK1, baud);
1719 USART3_BASE->CR3 &= ~USART_CR3_EIE & ~USART_CR3_CTSIE; // Disable receive
1720 USART3_BASE->CR1 &= ~USART_CR1_RE & ~USART_CR1_RXNEIE & ~USART_CR1_PEIE & ~USART_CR1_IDLEIE ; // Disable RX and interrupts
1721 USART3_BASE->CR1 |= (USART_CR1_TE | USART_CR1_UE); // Enable USART3 and TX
1722 }
1723 void init_HWTimer()
1724 {
1725 HWTimer2.pause(); // Pause the timer2 while we're configuring it
1726 TIMER2_BASE->PSC = 35; // 36-1;for 72 MHZ /0.5sec/(35+1)
1727 TIMER2_BASE->ARR = 0xFFFF; // Count until 0xFFFF
1728 HWTimer2.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); // Main scheduler
1729 TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer2/Comp2 interrupt flag
1730 TIMER2_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer2/Comp2 interrupt
1731 HWTimer2.refresh(); // Refresh the timer's count, prescale, and overflow
1732 HWTimer2.resume();
1734 #ifdef ENABLE_SERIAL
1735 HWTimer3.pause(); // Pause the timer3 while we're configuring it
1736 TIMER3_BASE->PSC = 35; // 36-1;for 72 MHZ /0.5sec/(35+1)
1737 TIMER3_BASE->ARR = 0xFFFF; // Count until 0xFFFF
1738 HWTimer3.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE); // Serial check
1739 TIMER3_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer3/Comp2 interrupt flag
1740 HWTimer3.attachInterrupt(TIMER_CH2,ISR_COMPB); // Assign function to Timer3/Comp2 interrupt
1741 TIMER3_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer3/Comp2 interrupt
1742 HWTimer3.refresh(); // Refresh the timer's count, prescale, and overflow
1743 HWTimer3.resume();
1744 #endif
1745 }
1746 #endif
1748 #ifdef CHECK_FOR_BOOTLOADER
1749 void pollBoot()
1750 {
1751 uint8_t rxchar ;
1752 uint8_t lState = BootState ;
1753 uint8_t millisTime = millis(); // Call this once only
1755 #ifdef ORANGE_TX
1756 if ( USARTC0.STATUS & USART_RXCIF_bm )
1757 #elif defined STM32_BOARD
1758 if ( USART2_BASE->SR & USART_SR_RXNE )
1759 #else
1760 if ( UCSR0A & ( 1 << RXC0 ) )
1761 #endif
1762 {
1763 rxchar = UDR0 ;
1764 BootCount += 1 ;
1765 if ( ( lState == BOOT_WAIT_30_IDLE ) || ( lState == BOOT_WAIT_30_DATA ) )
1766 {
1767 if ( lState == BOOT_WAIT_30_IDLE ) // Waiting for 0x30
1768 BootTimer = millisTime ; // Start timeout
1769 if ( rxchar == 0x30 )
1770 lState = BOOT_WAIT_20 ;
1771 else
1772 lState = BOOT_WAIT_30_DATA ;
1773 }
1774 else
1775 if ( lState == BOOT_WAIT_20 && rxchar == 0x20 ) // Waiting for 0x20
1776 lState = BOOT_READY ;
1777 }
1778 else // No byte received
1779 {
1780 if ( lState != BOOT_WAIT_30_IDLE ) // Something received
1781 {
1782 uint8_t time = millisTime - BootTimer ;
1783 if ( time > 5 )
1784 {
1785 #ifdef STM32_BOARD
1786 if ( BootCount > 4 )
1787 #else
1788 if ( BootCount > 2 )
1789 #endif
1790 { // Run normally
1791 NotBootChecking = 0xFF ;
1792 Mprotocol_serial_init( 0 ) ;
1793 }
1794 else if ( lState == BOOT_READY )
1795 {
1796 #ifdef STM32_BOARD
1797 nvic_sys_reset();
1798 while(1); /* wait until reset */
1799 #else
1800 cli(); // Disable global int due to RW of 16 bits registers
1801 void (*p)();
1802 #ifndef ORANGE_TX
1803 p = (void (*)())0x3F00 ; // Word address (0x7E00 byte)
1804 #else
1805 p = (void (*)())0x4000 ; // Word address (0x8000 byte)
1806 #endif
1807 (*p)() ; // go to boot
1808 #endif
1809 }
1810 else
1811 {
1812 lState = BOOT_WAIT_30_IDLE ;
1813 BootCount = 0 ;
1814 }
1815 }
1816 }
1817 }
1818 BootState = lState ;
1819 }
1820 #endif //CHECK_FOR_BOOTLOADER
1822 #if defined(TELEMETRY)
1823 void PPM_Telemetry_serial_init()
1824 {
1825 if( (protocol==PROTO_FRSKYD) || (protocol==PROTO_HUBSAN) || (protocol==PROTO_AFHDS2A) || (protocol==PROTO_BAYANG)|| (protocol==PROTO_NCC1701) || (protocol==PROTO_CABELL) || (protocol==PROTO_HITEC) || (protocol==PROTO_BUGS) || (protocol==PROTO_BUGSMINI) || (protocol==PROTO_PROPEL) || (protocol==PROTO_OMP) || (protocol==PROTO_RLINK) || (protocol==PROTO_WFLY2) || (protocol==PROTO_LOLI) || (protocol==PROTO_MT99XX)
1826 #ifdef TELEMETRY_FRSKYX_TO_FRSKYD
1827 || (protocol==PROTO_FRSKYX) || (protocol==PROTO_FRSKYX2)
1828 #endif
1829 )
1830 initTXSerial( SPEED_9600 ) ;
1831 #ifndef TELEMETRY_FRSKYX_TO_FRSKYD
1832 if(protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2)
1833 initTXSerial( SPEED_57600 ) ;
1834 #endif
1835 if(protocol==PROTO_DSM)
1836 initTXSerial( SPEED_125K ) ;
1837 }
1838 #endif
1840 // Convert 32b id to rx_tx_addr
1841 static void set_rx_tx_addr(uint32_t id)
1842 { // Used by almost all protocols
1843 rx_tx_addr[0] = (id >> 24) & 0xFF;
1844 rx_tx_addr[1] = (id >> 16) & 0xFF;
1845 rx_tx_addr[2] = (id >> 8) & 0xFF;
1846 rx_tx_addr[3] = (id >> 0) & 0xFF;
1847 rx_tx_addr[4] = (rx_tx_addr[2]&0xF0)|(rx_tx_addr[3]&0x0F);
1848 }
1850 static uint32_t random_id(uint16_t address, uint8_t create_new)
1851 {
1852 #ifndef FORCE_GLOBAL_ID
1853 uint32_t id=0;
1855 if(eeprom_read_byte((EE_ADDR)(address+10))==0xf0 && !create_new)
1856 { // TXID exists in EEPROM
1857 for(uint8_t i=4;i>0;i--)
1858 {
1859 id<<=8;
1860 id|=eeprom_read_byte((EE_ADDR)address+i-1);
1861 }
1862 if(id!=0x2AD141A7) //ID with seed=0
1863 {
1864 //debugln("Read ID from EEPROM");
1865 return id;
1866 }
1867 }
1868 // Generate a random ID
1869 #if defined STM32_BOARD
1870 #define STM32_UUID ((uint32_t *)0x1FFFF7E8)
1871 if (!create_new)
1872 {
1873 id = STM32_UUID[0] ^ STM32_UUID[1] ^ STM32_UUID[2];
1874 debugln("Generated ID from STM32 UUID");
1875 }
1876 else
1877 #endif
1878 id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
1880 for(uint8_t i=0;i<4;i++)
1881 eeprom_write_byte((EE_ADDR)address+i,id >> (i*8));
1882 eeprom_write_byte((EE_ADDR)(address+10),0xf0);//write bind flag in eeprom.
1883 return id;
1884 #else
1885 (void)address;
1886 (void)create_new;
1887 return FORCE_GLOBAL_ID;
1888 #endif
1889 }
1891 // Generate frequency hopping sequence in the range [02..77]
1892 static void __attribute__((unused)) calc_fh_channels(uint8_t num_ch)
1893 {
1894 uint8_t idx = 0;
1895 uint32_t rnd = MProtocol_id;
1896 uint8_t max=(num_ch/3)+2;
1898 while (idx < num_ch)
1899 {
1900 uint8_t i;
1901 uint8_t count_2_26 = 0, count_27_50 = 0, count_51_74 = 0;
1903 rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization
1904 // Use least-significant byte. 73 is prime, so channels 76..77 are unused
1905 uint8_t next_ch = ((rnd >> 8) % 73) + 2;
1906 // Keep a distance of 5 between consecutive channels
1907 if (idx !=0)
1908 {
1909 if(hopping_frequency[idx-1]>next_ch)
1910 {
1911 if(hopping_frequency[idx-1]-next_ch<5)
1912 continue;
1913 }
1914 else
1915 if(next_ch-hopping_frequency[idx-1]<5)
1916 continue;
1917 }
1918 // Check that it's not duplicated and spread uniformly
1919 for (i = 0; i < idx; i++) {
1920 if(hopping_frequency[i] == next_ch)
1921 break;
1922 if(hopping_frequency[i] <= 26)
1923 count_2_26++;
1924 else if (hopping_frequency[i] <= 50)
1925 count_27_50++;
1926 else
1927 count_51_74++;
1928 }
1929 if (i != idx)
1930 continue;
1931 if ( (next_ch <= 26 && count_2_26 < max) || (next_ch >= 27 && next_ch <= 50 && count_27_50 < max) || (next_ch >= 51 && count_51_74 < max) )
1932 hopping_frequency[idx++] = next_ch;//find hopping frequency
1933 }
1934 }
1936 static uint8_t __attribute__((unused)) bit_reverse(uint8_t b_in)
1937 {
1938 uint8_t b_out = 0;
1939 for (uint8_t i = 0; i < 8; ++i)
1940 {
1941 b_out = (b_out << 1) | (b_in & 1);
1942 b_in >>= 1;
1943 }
1944 return b_out;
1945 }
1947 static void __attribute__((unused)) crc16_update(uint8_t a, uint8_t bits)
1948 {
1949 crc ^= a << 8;
1950 while(bits--)
1951 if (crc & 0x8000)
1952 crc = (crc << 1) ^ crc16_polynomial;
1953 else
1954 crc = crc << 1;
1955 }
1957 static void __attribute__((unused)) crc8_update(uint8_t byte)
1958 {
1959 crc8 = crc8 ^ byte;
1960 for ( uint8_t j = 0; j < 8; j++ )
1961 if ( crc8 & 0x80 )
1962 crc8 = (crc8<<1) ^ crc8_polynomial;
1963 else
1964 crc8 <<= 1;
1965 }
1967 /**************************/
1968 /**************************/
1969 /** Interrupt routines **/
1970 /**************************/
1971 /**************************/
1973 //PPM
1974 #ifdef ENABLE_PPM
1975 #ifdef ORANGE_TX
1976 #if PPM_pin == 2
1977 ISR(PORTD_INT0_vect)
1978 #else
1979 ISR(PORTD_INT1_vect)
1980 #endif
1981 #elif defined STM32_BOARD
1982 void PPM_decode()
1983 #else
1984 #if PPM_pin == 2
1985 ISR(INT0_vect, ISR_NOBLOCK)
1986 #else
1987 ISR(INT1_vect, ISR_NOBLOCK)
1988 #endif
1989 #endif
1990 { // Interrupt on PPM pin
1991 static int8_t chan=0,bad_frame=1;
1992 static uint16_t Prev_TCNT1=0;
1993 uint16_t Cur_TCNT1;
1995 Cur_TCNT1 = TCNT1 - Prev_TCNT1 ; // Capture current Timer1 value
1996 if(Cur_TCNT1<1600)
1997 bad_frame=1; // bad frame
1998 else
1999 if(Cur_TCNT1>4400)
2000 { //start of frame
2001 if(chan>=MIN_PPM_CHANNELS)
2002 {
2003 PPM_FLAG_on; // good frame received if at least 4 channels have been seen
2004 if(chan>PPM_chan_max) PPM_chan_max=chan; // Saving the number of channels received
2005 }
2006 chan=0; // reset channel counter
2007 bad_frame=0;
2008 }
2009 else
2010 if(bad_frame==0) // need to wait for start of frame
2011 { //servo values between 800us and 2200us will end up here
2012 PPM_data[chan]=Cur_TCNT1;
2013 if(chan++>=MAX_PPM_CHANNELS)
2014 bad_frame=1; // don't accept any new channels
2015 }
2016 Prev_TCNT1+=Cur_TCNT1;
2017 }
2018 #endif //ENABLE_PPM
2020 //Serial RX
2021 #ifdef ENABLE_SERIAL
2022 #ifdef ORANGE_TX
2023 ISR(USARTC0_RXC_vect)
2024 #elif defined STM32_BOARD
2025 void __irq_usart2()
2026 #else
2027 ISR(USART_RX_vect)
2028 #endif
2029 { // RX interrupt
2030 #ifdef ORANGE_TX
2031 if((USARTC0.STATUS & 0x1C)==0) // Check frame error, data overrun and parity error
2032 #elif defined STM32_BOARD
2033 if((USART2_BASE->SR & USART_SR_RXNE) && (USART2_BASE->SR &0x0F)==0)
2034 #else
2035 UCSR0B &= ~_BV(RXCIE0) ; // RX interrupt disable
2036 sei() ;
2037 if((UCSR0A&0x1C)==0) // Check frame error, data overrun and parity error
2038 #endif
2039 { // received byte is ok to process
2040 if(rx_idx==0||discard_frame==true)
2041 { // Let's try to sync at this point
2042 RX_MISSED_BUFF_off; // If rx_buff was good it's not anymore...
2043 rx_idx=0;discard_frame=false;
2044 rx_buff[0]=UDR0;
2045 #ifdef FAILSAFE_ENABLE
2046 if((rx_buff[0]&0xFC)==0x54) // If 1st byte is 0x54, 0x55, 0x56 or 0x57 it looks ok
2047 #else
2048 if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
2049 #endif
2050 {
2051 #if defined STM32_BOARD
2052 TIMER3_BASE->CCR2=TIMER3_BASE->CNT + 500; // Next byte should show up within 250us (1 byte = 120us)
2053 TIMER3_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer3/Comp2 interrupt flag
2054 TIMER3_BASE->DIER |= TIMER_DIER_CC2IE; // Enable Timer3/Comp2 interrupt
2055 #else
2056 TX_RX_PAUSE_on;
2057 tx_pause();
2058 cli(); // Disable global int due to RW of 16 bits registers
2059 OCR1B = TCNT1 + 500; // Next byte should show up within 250us (1 byte = 120us)
2060 sei(); // Enable global int
2061 TIFR1 = OCF1B_bm ; // clear OCR1B match flag
2062 SET_TIMSK1_OCIE1B ; // enable interrupt on compare B match
2063 #endif
2064 rx_idx++;
2065 }
2066 }
2067 else
2068 {
2069 if(rx_idx>=RXBUFFER_SIZE)
2070 {
2071 discard_frame=true; // Too many bytes being received...
2072 debugln("RX frame too long");
2073 }
2074 else
2075 {
2076 rx_buff[rx_idx++]=UDR0; // Store received byte
2077 #if defined STM32_BOARD
2078 TIMER3_BASE->CCR2=TIMER3_BASE->CNT + 500; // Next byte should show up within 250us (1 byte = 120us)
2079 #else
2080 cli(); // Disable global int due to RW of 16 bits registers
2081 OCR1B = TCNT1 + 500; // Next byte should show up within 250us (1 byte = 120us)
2082 sei(); // Enable global int
2083 #endif
2084 }
2085 }
2086 }
2087 else
2088 {
2089 rx_idx=UDR0; // Dummy read
2090 rx_idx=0;
2091 discard_frame=true; // Error encountered discard full frame...
2092 debugln("Bad frame RX");
2093 }
2094 if(discard_frame==true)
2095 {
2096 #ifdef STM32_BOARD
2097 TIMER3_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer3/Comp2 interrupt
2098 #else
2099 CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
2100 TX_RX_PAUSE_off;
2101 tx_resume();
2102 #endif
2103 }
2104 #if not defined (ORANGE_TX) && not defined (STM32_BOARD)
2105 cli() ;
2106 UCSR0B |= _BV(RXCIE0) ; // RX interrupt enable
2107 #endif
2108 }
2110 //Serial timer
2111 #ifdef ORANGE_TX
2112 ISR(TCC1_CCB_vect)
2113 #elif defined STM32_BOARD
2114 void ISR_COMPB()
2115 #else
2116 ISR(TIMER1_COMPB_vect)
2117 #endif
2118 { // Timer1 compare B interrupt
2119 if(rx_idx>=26 && rx_idx<=RXBUFFER_SIZE)
2120 {
2121 // A full frame has been received
2122 if(!IS_RX_DONOTUPDATE_on)
2123 { //Good frame received and main is not working on the buffer
2124 rx_len=rx_idx;
2125 memcpy((void*)rx_ok_buff,(const void*)rx_buff,rx_idx); // Duplicate the buffer
2126 RX_FLAG_on; // Flag for main to process data
2127 }
2128 else
2129 RX_MISSED_BUFF_on; // Notify that rx_buff is good
2130 #ifdef MULTI_SYNC
2131 cli();
2132 last_serial_input=TCNT1;
2133 sei();
2134 #endif
2135 }
2136 #ifdef DEBUG_SERIAL
2137 else
2138 debugln("RX frame size incorrect");
2139 #endif
2140 discard_frame=true;
2141 #ifdef STM32_BOARD
2142 TIMER3_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer3/Comp2 interrupt
2143 #else
2144 CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
2145 TX_RX_PAUSE_off;
2146 tx_resume();
2147 #endif
2148 }
2149 #endif //ENABLE_SERIAL
2151 /**************************/
2152 /**************************/
2153 /** CPPM routines **/
2154 /**************************/
2155 /**************************/
2156 #ifdef SEND_CPPM
2157 #define PPM_CENTER 1500*2
2158 uint32_t TrainerTimer ;
2159 bool CppmInitialised = false;
2160 uint16_t *TrainerPulsePtr ;
2161 uint16_t TrainerPpmStream[10] ;
2162 int16_t CppmChannels[8] ;
2164 void setupTrainerPulses()
2165 {
2166 uint32_t i ;
2167 uint32_t total ;
2168 uint32_t pulse ;
2169 uint16_t *ptr ;
2170 uint32_t p = 8 ;
2171 int16_t PPM_range = 512*2 ; //range of 0.7..1.7msec
2173 ptr = TrainerPpmStream ;
2175 total = 22500u*2; //Minimum Framelen=22.5 ms
2177 if ( (millis() - TrainerTimer) < 400 )
2178 {
2179 for ( i = 0 ; i < p ; i += 1 )
2180 {
2181 pulse = max( (int)min(CppmChannels[i],PPM_range),-PPM_range) + PPM_CENTER ;
2183 total -= pulse ;
2184 *ptr++ = pulse ;
2185 }
2186 }
2187 *ptr++ = total ;
2188 *ptr = 0 ;
2189 TIMER1_BASE->CCR1 = total - 1500 ; // Update time
2190 TIMER1_BASE->CCR2 = 300*2 ;
2191 }
2193 void init_trainer_ppm()
2194 {
2195 // Timer 1, channel 2 on PA9
2196 RCC_BASE->APB2ENR |= RCC_APB2ENR_TIM1EN ; // Enable clock
2197 setupTrainerPulses() ;
2198 RCC_BASE->APB2ENR |= RCC_APB2ENR_IOPAEN ; // Enable portA clock
2199 RCC_BASE->APB2ENR &= ~RCC_APB2ENR_USART1EN ; // Disable USART1
2201 GPIOA_BASE->CRH &= ~0x00F0 ;
2202 GPIOA_BASE->CRH |= 0x00A0 ; // AF PP OP2MHz
2204 HWTimer1.pause() ; // Pause the timer1 while we're configuring it
2205 TIMER1_BASE->ARR = *TrainerPulsePtr++ ;
2206 TIMER1_BASE->PSC = 72000000 / 2000000 - 1 ; // 0.5uS
2207 TIMER1_BASE->CCR2 = 600 ; // 300 uS pulse
2208 TIMER1_BASE->CCR1 = 5000 ; // 2500 uS pulse
2209 TIMER1_BASE->CCMR1 = 0x6000 ; // PWM mode 1 (header file has incorrect bits)
2210 TIMER1_BASE->EGR = 1 ;
2211 TIMER1_BASE->CCER = TIMER_CCER_CC2E ;
2212 TIMER1_BASE->DIER |= TIMER_DIER_UIE ;
2213 TIMER1_BASE->CR1 = TIMER_CR1_CEN ;
2214 nvic_irq_set_priority(NVIC_TIMER1_CC, 4 ) ;
2215 nvic_irq_set_priority(NVIC_TIMER1_UP, 4 ) ;
2216 HWTimer1.attachInterrupt(TIMER_UPDATE_INTERRUPT,tim1_up); // Assign function to Timer1/Comp2 interrupt
2217 HWTimer1.attachInterrupt(TIMER_CH1,tim1_cc); // Assign function to Timer1/Comp2 interrupt
2219 CppmInitialised = true ;
2220 HWTimer1.resume() ;
2221 }
2223 void release_trainer_ppm()
2224 {
2225 if ( CppmInitialised )
2226 {
2227 TIMER1_BASE->CR1 = 0 ;
2228 pinMode(PA9,INPUT) ;
2229 CppmInitialised = false ;
2230 }
2231 }
2233 void tim1_up()
2234 {
2235 #define TIMER1_SR_MASK 0x1FFF
2236 // PPM out update interrupt
2237 if ( (TIMER1_BASE->DIER & TIMER_DIER_UIE) && ( TIMER1_BASE->SR & TIMER_SR_UIF ) )
2238 {
2239 GPIOA_BASE->BRR = 0x0200 ;
2240 TIMER1_BASE->SR = TIMER1_SR_MASK & ~TIMER_SR_UIF ; // Clear flag
2241 TIMER1_BASE->ARR = *TrainerPulsePtr++ ;
2242 if ( *TrainerPulsePtr == 0 )
2243 {
2244 TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_CC1IF ; // Clear this flag
2245 TIMER1_BASE->DIER |= TIMER_DIER_CC1IE ; // Enable this interrupt
2246 TIMER1_BASE->DIER &= ~TIMER_DIER_UIE ; // Stop this interrupt
2247 }
2248 }
2249 }
2251 void tim1_cc()
2252 {
2253 if ( ( TIMER1_BASE->DIER & TIMER_DIER_CC1IE ) && ( TIMER1_BASE->SR & TIMER_SR_CC1IF ) )
2254 {
2255 // compare interrupt
2256 TIMER1_BASE->DIER &= ~TIMER_DIER_CC1IE ; // Stop this interrupt
2257 TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_CC1IF ; // Clear flag
2259 setupTrainerPulses() ;
2261 TrainerPulsePtr = TrainerPpmStream ;
2262 TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_UIF ; // Clear this flag
2263 TIMER1_BASE->DIER |= TIMER_DIER_UIE ; // Enable this interrupt
2264 }
2265 }
2267 void Send_CCPM_USART1()
2268 {
2269 if ( CppmInitialised == false )
2270 init_trainer_ppm() ;
2271 TrainerTimer = millis() ;
2272 len = packet_in[3] ;
2273 uint32_t bitsavailable = 0 ;
2274 uint32_t bits = 0 ; ;
2275 uint32_t i ;
2276 int16_t value ;
2277 uint8_t *packet ;
2278 packet = &packet_in[4] ;
2279 i = packet_in[2] ; // Start channel
2280 // Load changed channels
2281 while ( len )
2282 {
2283 while ( bitsavailable < 11 )
2284 {
2285 bits |= *packet++ << bitsavailable ;
2286 bitsavailable += 8 ;
2287 }
2288 value = bits & 0x07FF ;
2289 value -= 0x0400 ;
2290 bitsavailable -= 11 ;
2291 bits >>= 11 ;
2292 if ( i < 8 )
2293 CppmChannels[i] = value * 5 / 4 ;
2294 i++ ;
2295 len-- ;
2296 }
2297 }
2298 #endif
2300 /**************************/
2301 /**************************/
2302 /** Arduino random **/
2303 /**************************/
2304 /**************************/
2305 #if not defined (ORANGE_TX) && not defined (STM32_BOARD)
2306 static void random_init(void)
2307 {
2308 cli(); // Temporarily turn off interrupts, until WDT configured
2309 MCUSR = 0; // Use the MCU status register to reset flags for WDR, BOR, EXTR, and POWR
2310 WDTCSR |= _BV(WDCE); // WDT control register, This sets the Watchdog Change Enable (WDCE) flag, which is needed to set the prescaler
2311 WDTCSR = _BV(WDIE); // Watchdog interrupt enable (WDIE)
2312 sei(); // Turn interupts on
2313 }
2315 static uint32_t random_value(void)
2316 {
2317 while (!gWDT_entropy);
2318 return gWDT_entropy;
2319 }
2321 // Random interrupt service routine called every time the WDT interrupt is triggered.
2322 // It is only enabled at startup to generate a seed.
2323 ISR(WDT_vect)
2324 {
2325 static uint8_t gWDT_buffer_position=0;
2326 #define gWDT_buffer_SIZE 32
2327 static uint8_t gWDT_buffer[gWDT_buffer_SIZE];
2328 gWDT_buffer[gWDT_buffer_position] = TCNT1L; // Record the Timer 1 low byte (only one needed)
2329 gWDT_buffer_position++; // every time the WDT interrupt is triggered
2330 if (gWDT_buffer_position >= gWDT_buffer_SIZE)
2331 {
2332 // The following code is an implementation of Jenkin's one at a time hash
2333 for(uint8_t gWDT_loop_counter = 0; gWDT_loop_counter < gWDT_buffer_SIZE; ++gWDT_loop_counter)
2334 {
2335 gWDT_entropy += gWDT_buffer[gWDT_loop_counter];
2336 gWDT_entropy += (gWDT_entropy << 10);
2337 gWDT_entropy ^= (gWDT_entropy >> 6);
2338 }
2339 gWDT_entropy += (gWDT_entropy << 3);
2340 gWDT_entropy ^= (gWDT_entropy >> 11);
2341 gWDT_entropy += (gWDT_entropy << 15);
2342 WDTCSR = 0; // Disable Watchdog interrupt
2343 }
2344 }
2345 #endif