gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-02-10 01:18:15 +00:00
Code Issues Projects Releases Wiki Activity
DIY-Multiprotocol-TX-Module/Multiprotocol/Multiprotocol.ino
Dennis cec654a75b Update Protocol Details for CABELL protocol (#117) 
* Added CABELL Protocol

* Added additional disclaimer to license.

* Revert "Added additional disclaimer to license."

This reverts commit a00bc9956a155a1a73dbbcd21fd19633d6d121ff.

* Added additional disclaimer

* Added CABELL_NRF24L01_INO define to config

* Updated available protocol list

Added CABELL protocol

* Removed unused variables

* Changed changel range to 45 channels that comply with USA FCC part 97 rules.

This change allows licenced HAMs to operate under part 97 rules instead
of part 15.  These channels are still in the ISM band, but overlap with
the part 97 amateur portion of the band.

* Changed protocol number to 33

Was previously 30, but the main branch has now allocated up to 32, so
changing to 33

* Corrected permutation calculation

* Added sub-protocol for setting failsafe values

* Opened up a free bit in the option byte for future use

* Fixed packet errors when trying to unbind when in bind mode

This use case didn't really make any sense, but it should not cause
packet errors, so fixed it.

* RSSI Telemetry for CABELL protocol

* Pins back to stock configuration

* Split checksum into MSB and LSB fields to avoid endian issue

* struct change for checksum

* Added analog values to telemetry packet that could be used for LIPO voltage

* Added MODE_CABELL to frsk_link_frame

* Updated packet layout comments

* Fixed telemetry conditional compiles in CABELL protocol

* Telemetry working; moved power override bit

* Changed telemetry to 250 kbps and adjustable packet period - imporves reliability/range

* Changed CABELL protocol number to 34

* Fixed typos in comments

* Fix ATMEGA BASH_SERIAL buffer overrun

Changed the compare to TXBUFFER_SIZE to >=
If next wasn't set to zero until > TXBUFFER_SIZE then the next time the
routines get called the the array index references outside the buffer
(e.g tail+1)

* Revert "Fix ATMEGA BASH_SERIAL buffer overrun"

This reverts commit ba4526ee89c2b5a946a659eea2c9581c36254809.

* Updated documentation for CABELL V3 Protocol

* Updated Documentation for the CABELL V3 Protocol
2017-12-01 14:13:06 +01:00

1592 lines
44 KiB
C++
Raw Blame History

/*********************************************************
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 <http://www.gnu.org/licenses/>.
*/
#include <avr/pgmspace.h>
//#define DEBUG_TX
//#define SERIAL_DEBUG // Only for STM32_BOARD compiled with Upload method "Serial"->usart1, "STM32duino bootloader"->USB serial
#ifdef __arm__// Let's automatically select the board if arm is selected
#define STM32_BOARD
#endif
#ifdef ARDUINO_AVR_XMEGA32D4
#include "MultiOrange.h"
#endif
#include "Multiprotocol.h"
//Multiprotocol module configuration file
#include "_Config.h"
//Personal config file
#if __has_include("_MyConfig.h")
#include "_MyConfig.h"
#endif
#include "Pins.h"
#include "TX_Def.h"
#include "Validate.h"
#ifndef STM32_BOARD
#include <avr/eeprom.h>
#else
#include <arduino.h>
#include <libmaple/usart.h>
#include <libmaple/timer.h>
#include <SPI.h>
#include <EEPROM.h>
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 // TELEMETRY
// 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()
{
// Setup diagnostic uart before anything else
#ifdef SERIAL_DEBUG
Serial.begin(115200,SERIAL_8N1);
while (!Serial); // Wait for ever for the serial port to connect...
debug("Multiprotocol version: %d.%d.%d.%d", VERSION_MAJOR, VERSION_MINOR, VERSION_REVISION, VERSION_PATCH_LEVEL);
#endif
// 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(SPI_CSN_pin,OUTPUT);
pinMode(CYRF_RST_pin,OUTPUT);
pinMode(PE1_pin,OUTPUT);
pinMode(PE2_pin,OUTPUT);
pinMode(TX_INV_pin,OUTPUT);
pinMode(RX_INV_pin,OUTPUT);
#if defined TELEMETRY
#if defined INVERT_SERIAL
TX_INV_on; //activate 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;i<NUM_CHN;i++)
Servo_data[i]=1500;
Servo_data[THROTTLE]=servo_min_100;
#ifdef ENABLE_PPM
memcpy((void *)PPM_data,Servo_data, sizeof(Servo_data));
#endif
//Wait for every component to start
delayMilliseconds(100);
// Read status of bind button
if( IS_BIND_BUTTON_on )
BIND_BUTTON_FLAG_on; // If bind button pressed save the status for protocol id reset under hubsan
// Read status of mode select binary switch
// after this mode_select will be one of {0000, 0001, ..., 1111}
#ifndef ENABLE_PPM
mode_select = MODE_SERIAL ; // force serial mode
#elif defined STM32_BOARD
mode_select= 0x0F -(uint8_t)(((GPIOA->regs->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
debug("Mode switch reads as %d", mode_select);
// 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);
debug("Module Id: %lx", MProtocol_id_master);
#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
debug("Init complete");
}
// 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;i<MAX_PPM_CHANNELS;i++)
{ // update servo data without interrupts to prevent bad read in protocols
uint16_t temp_ppm ;
cli(); // disable global int
temp_ppm = PPM_data[i] ;
sei(); // enable global int
if(temp_ppm<PPM_MIN_125) temp_ppm=PPM_MIN_125;
else if(temp_ppm>PPM_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) || (protocol==MODE_CABELL) )
#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]<PPM_MIN_COMMAND)
{
BIND_CH_PREV_off;
#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;
}
#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<<i;
}
// Update led status based on binding and serial
static void update_led_status(void)
{
if(IS_INPUT_SIGNAL_on)
if(millis()-last_signal>70)
INPUT_SIGNAL_off; //no valid signal (PPM or Serial) received for 70ms
if(blink<millis())
{
if(IS_INPUT_SIGNAL_off)
{
if(mode_select==MODE_SERIAL)
blink+=BLINK_SERIAL_TIME; //blink slowly if no valid serial input
else
blink+=BLINK_PPM_TIME; //blink more slowly if no valid PPM input
}
else
if(remote_callback == 0)
{ // Invalid protocol
if(IS_LED_on) //flash to indicate invalid protocol
blink+=BLINK_BAD_PROTO_TIME_LOW;
else
blink+=BLINK_BAD_PROTO_TIME_HIGH;
}
else
{
if(IS_WAIT_BIND_on)
{
if(IS_LED_on) //flash to indicate WAIT_BIND
blink+=BLINK_WAIT_BIND_TIME_LOW;
else
blink+=BLINK_WAIT_BIND_TIME_HIGH;
}
else
{
if(IS_BIND_DONE_on)
LED_off; //bind completed force led on
blink+=BLINK_BIND_TIME; //blink fastly during binding
}
}
LED_toggle;
}
}
inline void tx_pause()
{
#ifdef TELEMETRY
// Pause telemetry by disabling transmitter interrupt
#ifdef ORANGE_TX
USARTC0.CTRLA &= ~0x03 ;
#else
#ifndef BASH_SERIAL
#ifdef STM32_BOARD
USART3_BASE->CR1 &= ~ 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(EEPROM_ID_OFFSET,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)(MODELMODE_EEPROM_OFFSET+mode_select),0x00); // reset to autobind mode for the current model
option=0;
}
else
{
option=eeprom_read_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+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)(MODELMODE_EEPROM_OFFSET+mode_select),0x00); // reset to autobind mode for the current model
option=0;
}
else
{
option=eeprom_read_byte((EE_ADDR)(MODELMODE_EEPROM_OFFSET+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
#if defined(CABELL_NRF24L01_INO)
case MODE_CABELL:
next_callback=initCABELL();
remote_callback = CABELL_callback;
break;
#endif
#if defined(ESKY150_NRF24L01_INO)
case MODE_ESKY150:
next_callback=initESKY150();
remote_callback = ESKY150_callback;
break;
#endif
#if defined(H8_3D_NRF24L01_INO)
case MODE_H8_3D:
next_callback=initH8_3D();
remote_callback = H8_3D_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
debug("New protocol selected: %d, sub proto %d, rxnum %d", protocol, sub_protocol, RX_num);
}
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<NUM_CHN;i++)
{
dec+=3;
if(dec>=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
#endif // CHECK_FOR_BOOTLOADER
{
usart2_begin(100000,SERIAL_8E2);
USART2_BASE->CR1 |= USART_CR1_PCE_BIT;
}
usart3_begin(100000,SERIAL_8E2);
USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable
USART2_BASE->CR1 &= ~ USART_CR1_TE; //disable transmit
#else
//ATMEGA328p
#include <util/setbaud.h>
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
}
#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 && rxchar == 0x20 ) // Waiting for 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
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 //CHECK_FOR_BOOTLOADER
#if defined(TELEMETRY)
void PPM_Telemetry_serial_init()
{
if( (protocol==MODE_FRSKYD) || (protocol==MODE_HUBSAN) || (protocol==MODE_AFHDS2A) || (protocol==MODE_BAYANG) || (protocol==MODE_CABELL) )
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
Reference in New Issue View Git Blame Copy Permalink