gomaomao/DIY-Multiprotocol-TX-Module
1
0
Fork 0
mirror of https://github.com/pascallanger/DIY-Multiprotocol-TX-Module.git synced 2025-10-29 18:11:05 +00:00
Code Issues Projects Releases Wiki Activity
DIY-Multiprotocol-TX-Module/Multiprotocol/Multiprotocol.ino
pascallanger c193d0b9dd Update Multiprotocol.ino
2023-06-04 12:12:22 +02:00

2346 lines
64 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_PIN // Use pin TX for AVR and SPI_CS for STM32 => DEBUG_PIN_on, DEBUG_PIN_off, DEBUG_PIN_toggle
//#define DEBUG_SERIAL // 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
#if defined (ARDUINO_AVR_XMEGA32D4) || defined (ARDUINO_MULTI_ORANGERX)
#include "MultiOrange.h"
#endif
#include "Multiprotocol.h"
//Multiprotocol module configuration file
#include "_Config.h"
//Personal config file
#if defined(USE_MY_CONFIG)
#include "_MyConfig.h"
#endif
#include "Pins.h"
#include "TX_Def.h"
#include "Validate.h"
#ifndef STM32_BOARD
#include <avr/eeprom.h>
#else
#include <libmaple/usart.h>
#include <libmaple/timer.h>
//#include <libmaple/spi.h>
#include <SPI.h>
#include <EEPROM.h>
HardwareTimer HWTimer2(2);
#ifdef ENABLE_SERIAL
HardwareTimer HWTimer3(3);
void ISR_COMPB();
#endif
void PPM_decode();
extern "C"
{
void __irq_usart2(void);
void __irq_usart3(void);
}
#ifdef SEND_CPPM
HardwareTimer HWTimer1(1) ;
#endif
#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;
#if defined(ESKY150V2_CC2500_INO)
uint8_t packet[150];
#else
uint8_t packet[50];
#endif
#define NUM_CHN 16
// Servo data
uint16_t Channel_data[NUM_CHN];
uint8_t Channel_AUX;
#ifdef FAILSAFE_ENABLE
uint16_t Failsafe_data[NUM_CHN];
#endif
// Protocol variables
uint8_t cyrfmfg_id[6];//for dsm2 and devo
uint8_t rx_tx_addr[5];
uint8_t rx_id[5];
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;
#if defined(HOTT_CC2500_INO) || defined(ESKY150V2_CC2500_INO) || defined(MLINK_CYRF6936_INO)
uint8_t hopping_frequency[78];
#else
uint8_t hopping_frequency[50];
#endif
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;
uint16_t crc16_polynomial;
uint8_t crc8;
uint8_t crc8_polynomial;
uint16_t seed;
uint16_t failsafe_count;
uint16_t state;
uint8_t len;
uint8_t armed, arm_flags, arm_channel_previous;
uint8_t num_ch;
uint32_t pps_timer;
uint16_t pps_counter;
#ifdef CC2500_INSTALLED
#ifdef SCANNER_CC2500_INO
uint8_t calData[255];
#elif defined(HOTT_CC2500_INO) || defined(ESKY150V2_CC2500_INO)
uint8_t calData[75];
#else
uint8_t calData[50];
#endif
#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
uint8_t CH_AETR[]={AILERON, ELEVATOR, THROTTLE, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16};
uint8_t CH_TAER[]={THROTTLE, AILERON, ELEVATOR, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16};
//uint8_t CH_RETA[]={RUDDER, ELEVATOR, THROTTLE, AILERON, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16};
uint8_t CH_EATR[]={ELEVATOR, AILERON, THROTTLE, RUDDER, CH5, CH6, CH7, CH8, CH9, CH10, CH11, CH12, CH13, CH14, CH15, CH16};
// Mode_select variables
uint8_t mode_select;
uint8_t protocol_flags=0,protocol_flags2=0,protocol_flags3=0;
uint8_t option_override;
#ifdef ENABLE_PPM
// PPM variable
volatile uint16_t PPM_data[NUM_CHN];
volatile uint8_t PPM_chan_max=0;
uint32_t chan_order=0;
#endif
#if not defined (ORANGE_TX) && not defined (STM32_BOARD)
//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
uint8_t RX_num;
//Serial RX variables
#define BAUD 100000
#define RXBUFFER_SIZE 36 // 26+1+9
volatile uint8_t rx_buff[RXBUFFER_SIZE];
volatile uint8_t rx_ok_buff[RXBUFFER_SIZE];
volatile bool discard_frame = false;
volatile uint8_t rx_idx=0, rx_len=0;
// Callback
uint16_function_t remote_callback = 0;
// Telemetry
#define TELEMETRY_BUFFER_SIZE 32
uint8_t packet_in[TELEMETRY_BUFFER_SIZE];//telemetry receiving packets
#if defined(TELEMETRY)
#ifdef MULTI_SYNC
uint16_t last_serial_input=0;
uint16_t inputRefreshRate=0;
#endif
#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 telemetry_in_buffer[TELEMETRY_BUFFER_SIZE];//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;
#ifdef SPORT_SEND
#define MAX_SPORT_BUFFER 64
uint8_t SportData[MAX_SPORT_BUFFER];
uint8_t SportHead=0, SportTail=0;
#endif
// Functions definition when required
#ifdef HUB_TELEMETRY
static void __attribute__((unused)) frsky_send_user_frame(uint8_t, uint8_t, uint8_t);
#endif
//RX protocols
#if defined(AFHDS2A_RX_A7105_INO) || defined(FRSKY_RX_CC2500_INO) || defined(BAYANG_RX_NRF24L01_INO) || defined(DSM_RX_CYRF6936_INO)
bool rx_data_started;
bool rx_data_received;
bool rx_disable_lna;
uint16_t rx_rc_chan[16];
#endif
#ifdef HOTT_FW_TELEMETRY
uint8_t HoTT_SerialRX_val=0;
bool HoTT_SerialRX=false;
#endif
#ifdef DSM_FWD_PGM
uint8_t DSM_SerialRX_val[7];
bool DSM_SerialRX=false;
#endif
#ifdef MULTI_CONFIG_INO
uint8_t CONFIG_SerialRX_val[7];
bool CONFIG_SerialRX=false;
#endif
#endif // TELEMETRY
uint8_t multi_protocols_index=0xFF;
// Init
void setup()
{
// Setup diagnostic uart before anything else
#ifdef DEBUG_SERIAL
Serial.begin(115200,SERIAL_8N1);
// Wait up to 30s for a serial connection; double-blink the LED while we wait
unsigned long currMillis = millis();
unsigned long initMillis = currMillis;
pinMode(LED_pin,OUTPUT);
LED_off;
while (!Serial && (currMillis - initMillis) <= 30000) {
LED_on;
delay(100);
LED_off;
delay(100);
LED_on;
delay(100);
LED_off;
delay(500);
currMillis = millis();
}
delay(250); // Brief delay for FTDI debugging
debugln("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(LED_pin,OUTPUT);
pinMode(LED2_pin,OUTPUT);
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);
#ifdef MULTI_5IN1_INTERNAL
//pinMode(SX1276_RST_pin,OUTPUT); // already done by LED2_pin
pinMode(SX1276_TXEN_pin,OUTPUT); // PB0
pinMode(SX1276_DIO0_pin,INPUT_PULLUP);
#else
//Random pin
pinMode(RND_pin, INPUT_ANALOG); // set up PB0 pin for analog input
#endif
#if defined ENABLE_DIRECT_INPUTS
#if defined (DI1_PIN)
pinMode(DI1_PIN,INPUT_PULLUP);
#endif
#if defined (DI2_PIN)
pinMode(DI2_PIN,INPUT_PULLUP);
#endif
#if defined (DI3_PIN)
pinMode(DI3_PIN,INPUT_PULLUP);
#endif
#if defined (DI4_PIN)
pinMode(DI4_PIN,INPUT_PULLUP);
#endif
#endif
#ifdef SEND_CPPM
pinMode(PA9,INPUT); // make sure the USART1.TX pin is released for heartbeat use
#endif
//Timers
init_HWTimer(); //0.5us
//Read module flash size
#ifndef DISABLE_FLASH_SIZE_CHECK
unsigned short *flashSize = (unsigned short *) (0x1FFFF7E0);// Address register
debugln("Module Flash size: %dKB",(int)(*flashSize & 0xffff));
if((int)(*flashSize & 0xffff) < MCU_EXPECTED_FLASH_SIZE) // Not supported by this project
while (true) { //SOS
for(uint8_t i=0; i<3;i++)
{
LED_on;
delay(100);
LED_off;
delay(100);
}
for(uint8_t i=0; i<3;i++)
{
LED_on;
delay(500);
LED_off;
delay(100);
}
for(uint8_t i=0; i<3;i++)
{
LED_on;
delay(100);
LED_off;
delay(100);
}
LED_off;
delay(1000);
}
#endif
// Initialize the EEPROM
uint16_t eepromStatus = EEPROM.init();
debugln("EEPROM initialized: %d",eepromStatus);
// If there was no valid EEPROM page the EEPROM is corrupt or uninitialized and should be formatted
if( eepromStatus == EEPROM_NO_VALID_PAGE )
{
EEPROM.format();
debugln("No valid EEPROM page, EEPROM formatted");
}
#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
PROTO_DIAL1_port |= _BV(PROTO_DIAL1_pin);
PROTO_DIAL2_port |= _BV(PROTO_DIAL2_pin);
PROTO_DIAL3_port |= _BV(PROTO_DIAL3_pin);
PROTO_DIAL4_port |= _BV(PROTO_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
LED2_on;
// 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
#ifdef SPI_CSN_pin
SPI_CSN_on;
#endif
// Set SPI lines
#ifdef STM32_BOARD
initSPI2();
#else
SDI_on;
SCLK_off;
#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
BIND_IN_PROGRESS; // Request bind
}
else
BIND_DONE;
// 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 =
((PROTO_DIAL1_ipr & _BV(PROTO_DIAL1_pin)) ? 0 : 1) +
((PROTO_DIAL2_ipr & _BV(PROTO_DIAL2_pin)) ? 0 : 2) +
((PROTO_DIAL3_ipr & _BV(PROTO_DIAL3_pin)) ? 0 : 4) +
((PROTO_DIAL4_ipr & _BV(PROTO_DIAL4_pin)) ? 0 : 8);
#endif
//mode_select=1;
debugln("Protocol selection switch reads as %d", mode_select);
#ifdef ENABLE_PPM
uint8_t bank=bank_switch();
#endif
// Set default channels' value
for(uint8_t i=0;i<NUM_CHN;i++)
Channel_data[i]=1024;
Channel_data[THROTTLE]=0; //0=-125%, 204=-100%
#ifdef ENABLE_PPM
// Set default PPMs' value
for(uint8_t i=0;i<NUM_CHN;i++)
PPM_data[i]=PPM_MAX_100+PPM_MIN_100;
PPM_data[THROTTLE]=PPM_MIN_100*2;
#endif
// Update LED
LED_off;
LED_output;
//Init RF modules
modules_reset();
#ifndef ORANGE_TX
#ifdef STM32_BOARD
uint32_t seed=0;
for(uint8_t i=0;i<4;i++)
#ifdef RND_pin
seed=(seed<<8) | (analogRead(RND_pin)& 0xFF);
#else
//TODO find something to randomize...
seed=(seed<<8);
#endif
randomSeed(seed);
#else
//Init the seed with a random value created from watchdog timer for all protocols requiring random values
randomSeed(random_value());
#endif
#endif
// Read or create protocol id
MProtocol_id_master=random_id(EEPROM_ID_OFFSET,false);
debugln("Module Id: %lx", MProtocol_id_master);
#ifdef ENABLE_PPM
//Protocol and interrupts initialization
if(mode_select != MODE_SERIAL)
{ // PPM
#ifndef MY_PPM_PROT
const PPM_Parameters *PPM_prot_line=&PPM_prot[bank*14+mode_select-1];
#else
const PPM_Parameters *PPM_prot_line=&My_PPM_prot[bank*14+mode_select-1];
#endif
protocol = PPM_prot_line->protocol;
cur_protocol[1] = protocol;
sub_protocol = PPM_prot_line->sub_proto;
RX_num = PPM_prot_line->rx_num;
chan_order = PPM_prot_line->chan_order;
//Forced frequency tuning values for CC2500 protocols
#if defined(FORCE_FRSKYD_TUNING) && defined(FRSKYD_CC2500_INO)
if(protocol==PROTO_FRSKYD)
option = FORCE_FRSKYD_TUNING; // Use config-defined tuning value for FrSkyD
else
#endif
#if defined(FORCE_FRSKYL_TUNING) && defined(FRSKYL_CC2500_INO)
if(protocol==PROTO_FRSKYL)
option = FORCE_FRSKYL_TUNING; // Use config-defined tuning value for FrSkyL
else
#endif
#if defined(FORCE_FRSKYV_TUNING) && defined(FRSKYV_CC2500_INO)
if(protocol==PROTO_FRSKYV)
option = FORCE_FRSKYV_TUNING; // Use config-defined tuning value for FrSkyV
else
#endif
#if defined(FORCE_FRSKYX_TUNING) && defined(FRSKYX_CC2500_INO)
if(protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2)
option = FORCE_FRSKYX_TUNING; // Use config-defined tuning value for FrSkyX
else
#endif
#if defined(FORCE_FUTABA_TUNING) && defined(FUTABA_CC2500_INO)
if (protocol==PROTO_FUTABA)
option = FORCE_FUTABA_TUNING; // Use config-defined tuning value for SFHSS
else
#endif
#if defined(FORCE_CORONA_TUNING) && defined(CORONA_CC2500_INO)
if (protocol==PROTO_CORONA)
option = FORCE_CORONA_TUNING; // Use config-defined tuning value for CORONA
else
#endif
#if defined(FORCE_SKYARTEC_TUNING) && defined(SKYARTEC_CC2500_INO)
if (protocol==PROTO_SKYARTEC)
option = FORCE_SKYARTEC_TUNING; // Use config-defined tuning value for SKYARTEC
else
#endif
#if defined(FORCE_REDPINE_TUNING) && defined(REDPINE_CC2500_INO)
if (protocol==PROTO_REDPINE)
option = FORCE_REDPINE_TUNING; // Use config-defined tuning value for REDPINE
else
#endif
#if defined(FORCE_RADIOLINK_TUNING) && defined(RADIOLINK_CC2500_INO)
if (protocol==PROTO_RADIOLINK)
option = FORCE_RADIOLINK_TUNING; // Use config-defined tuning value for RADIOLINK
else
#endif
#if defined(FORCE_HITEC_TUNING) && defined(HITEC_CC2500_INO)
if (protocol==PROTO_HITEC)
option = FORCE_HITEC_TUNING; // Use config-defined tuning value for HITEC
else
#endif
#if defined(FORCE_HOTT_TUNING) && defined(HOTT_CC2500_INO)
if (protocol==PROTO_HOTT)
option = FORCE_HOTT_TUNING; // Use config-defined tuning value for HOTT
else
#endif
option = (uint8_t)PPM_prot_line->option; // Use radio-defined option value
if(PPM_prot_line->power) POWER_FLAG_on;
if(PPM_prot_line->autobind)
{
AUTOBIND_FLAG_on;
BIND_IN_PROGRESS; // Force a bind at protocol startup
}
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;
#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
}
debugln("Init complete");
LED2_on;
}
// Main
// Protocol scheduler
void loop()
{
uint16_t next_callback, diff;
uint8_t count=0;
while(1)
{
while(remote_callback==0 || IS_WAIT_BIND_on || IS_INPUT_SIGNAL_off)
if(!Update_All())
{
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
}
TX_MAIN_PAUSE_on;
tx_pause();
next_callback=remote_callback()<<1;
TX_MAIN_PAUSE_off;
tx_resume();
cli(); // Disable global int due to RW of 16 bits registers
OCR1A+=next_callback; // Calc when next_callback should happen
#ifndef STM32_BOARD
TIFR1=OCF1A_bm; // Clear compare A=callback flag
#else
TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag
#endif
diff=OCR1A-TCNT1; // Calc the time difference
sei(); // Enable global int
if((diff&0x8000) && !(next_callback&0x8000))
{ // Negative result=callback should already have been called...
debugln("Short CB:%d",next_callback);
}
else
{
if(IS_RX_FLAG_on || IS_PPM_FLAG_on)
{ // Serial or PPM is waiting...
if(++count>10)
{ //The protocol does not leave enough time for an update so forcing it
count=0;
debugln("Force update");
Update_All();
}
}
#ifndef STM32_BOARD
while((TIFR1 & OCF1A_bm) == 0)
#else
while((TIMER2_BASE->SR & TIMER_SR_CC1IF )==0)
#endif
{
if(diff>900*2)
{ //If at least 1ms is available update values
if((diff&0x8000) && !(next_callback&0x8000))
{//Should never get here...
debugln("!!!BUG!!!");
break;
}
count=0;
Update_All();
#ifdef DEBUG_SERIAL
if(TIMER2_BASE->SR & TIMER_SR_CC1IF )
debugln("Long update");
#endif
if(remote_callback==0)
break;
cli(); // Disable global int due to RW of 16 bits registers
diff=OCR1A-TCNT1; // Calc the time difference
sei(); // Enable global int
}
}
}
}
}
void End_Bind()
{
//Request protocol to terminate bind
if(protocol==PROTO_FRSKYD || protocol==PROTO_FRSKYL || protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || protocol==PROTO_FRSKYV || protocol==PROTO_FRSKY_R9
|| protocol==PROTO_DSM_RX || protocol==PROTO_AFHDS2A_RX || protocol==PROTO_FRSKY_RX || protocol==PROTO_BAYANG_RX
|| protocol==PROTO_AFHDS2A || protocol==PROTO_BUGS || protocol==PROTO_BUGSMINI || protocol==PROTO_HOTT || protocol==PROTO_ASSAN)
BIND_DONE;
else
if(bind_counter>2)
bind_counter=2;
}
void Update_Telem()
{
#if defined(TELEMETRY)
#ifndef MULTI_TELEMETRY
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))
#endif
if(IS_DISABLE_TELEM_off)
TelemetryUpdate();
#endif
}
bool 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
{
uint32_t chan_or=chan_order;
uint8_t ch;
uint8_t channelsCount = PPM_chan_max;
#ifdef ENABLE_DIRECT_INPUTS
#ifdef DI_CH1_read
PPM_data[channelsCount] = DI_CH1_read;
channelsCount++;
#endif
#ifdef DI_CH2_read
PPM_data[channelsCount] = DI_CH2_read;
channelsCount++;
#endif
#ifdef DI_CH3_read
PPM_data[channelsCount] = DI_CH3_read;
channelsCount++;
#endif
#ifdef DI_CH4_read
PPM_data[channelsCount] = DI_CH4_read;
channelsCount++;
#endif
#endif
for(uint8_t i=0;i<channelsCount;i++)
{ // update servo data without interrupts to prevent bad read
uint16_t val;
cli(); // disable global int
val = PPM_data[i];
sei(); // enable global int
val=map16b(val,PPM_MIN_100*2,PPM_MAX_100*2,CHANNEL_MIN_100,CHANNEL_MAX_100);
if(val&0x8000) val=CHANNEL_MIN_125;
else if(val>CHANNEL_MAX_125) val=CHANNEL_MAX_125;
if(chan_or)
{
ch=chan_or>>28;
if(ch)
Channel_data[ch-1]=val;
else
Channel_data[i]=val;
chan_or<<=4;
}
else
Channel_data[i]=val;
}
PPM_FLAG_off; // wait for next frame before update
#ifdef FAILSAFE_ENABLE
PPM_failsafe();
#endif
update_channels_aux();
INPUT_SIGNAL_on; // valid signal received
last_signal=millis();
}
#endif //ENABLE_PPM
update_led_status();
#ifdef SEND_CPPM
if ( telemetry_link & 0x80 )
{ // Protocol requests telemetry to be disabled
if( protocol == PROTO_FRSKY_RX || protocol == PROTO_AFHDS2A_RX || protocol == PROTO_BAYANG_RX || protocol == PROTO_DSM_RX )
{ // RX protocol
if(RX_LQI == 0)
telemetry_link = 0x00; // restore normal telemetry on connection loss
else if(telemetry_link & 1)
{ // New data available
Send_CCPM_USART1();
telemetry_link &= 0xFE; // update done
}
}
}
else
#endif
Update_Telem();
#ifdef ENABLE_BIND_CH
if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && Channel_data[BIND_CH-1]>CHANNEL_MAX_COMMAND)
{ // Autobind is on and BIND_CH went up
CHANGE_PROTOCOL_FLAG_on; // reload protocol
BIND_IN_PROGRESS; // enable bind
BIND_CH_PREV_on;
}
if(IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_on && Channel_data[BIND_CH-1]<CHANNEL_MIN_COMMAND)
{ // Autobind is on and BIND_CH went down
BIND_CH_PREV_off;
End_Bind();
}
#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 true;
}
return false;
}
#if defined(FAILSAFE_ENABLE) && defined(ENABLE_PPM)
void PPM_failsafe()
{
static uint8_t counter=0;
if(IS_BIND_IN_PROGRESS || IS_FAILSAFE_VALUES_on) // bind is not finished yet or Failsafe already being sent
return;
BIND_SET_INPUT;
BIND_SET_PULLUP;
if(IS_BIND_BUTTON_on)
{// bind button pressed
counter++;
if(counter>227)
{ //after 5s with PPM frames @22ms
counter=0;
for(uint8_t i=0;i<NUM_CHN;i++)
Failsafe_data[i]=Channel_data[i];
FAILSAFE_VALUES_on;
}
}
else
counter=0;
BIND_SET_OUTPUT;
}
#endif
// Update channels direction and Channel_AUX flags based on servo AUX positions
static void update_channels_aux(void)
{
//Reverse channels direction
#ifdef REVERSE_AILERON
reverse_channel(AILERON);
#endif
#ifdef REVERSE_ELEVATOR
reverse_channel(ELEVATOR);
#endif
#ifdef REVERSE_THROTTLE
reverse_channel(THROTTLE);
#endif
#ifdef REVERSE_RUDDER
reverse_channel(RUDDER);
#endif
//Calc AUX flags
Channel_AUX=0;
for(uint8_t i=0;i<8;i++)
if(Channel_data[CH5+i]>CHANNEL_SWITCH)
Channel_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
debugln("No input signal");
}
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)
LED_off; //bind completed force led on
blink+=BLINK_BIND_TIME; //blink fastly during binding
}
}
LED_toggle;
}
}
#ifdef ENABLE_PPM
uint8_t bank_switch(void)
{
uint8_t bank=eeprom_read_byte((EE_ADDR)EEPROM_BANK_OFFSET);
if(bank>=NBR_BANKS)
{ // Wrong number of bank
eeprom_write_byte((EE_ADDR)EEPROM_BANK_OFFSET,0x00); // set bank to 0
bank=0;
}
debugln("Using bank %d", bank);
phase=3;
uint32_t check=millis();
blink=millis();
while(mode_select==15)
{ //loop here if the dial is on position 15 for user to select the bank
if(blink<millis())
{
switch(phase & 0x03)
{ // Flash bank number of times
case 0:
LED_on;
blink+=BLINK_BANK_TIME_HIGH;
phase++;
break;
case 1:
LED_off;
blink+=BLINK_BANK_TIME_LOW;
phase++;
break;
case 2:
if( (phase>>2) >= bank)
{
phase=0;
blink+=BLINK_BANK_REPEAT;
}
else
phase+=2;
break;
case 3:
LED_output;
LED_off;
blink+=BLINK_BANK_TIME_LOW;
phase=0;
break;
}
}
if(check<millis())
{
//Test bind button: for AVR it's shared with the LED so some extra work is needed to check it...
#ifndef STM32_BOARD
bool led=IS_LED_on;
BIND_SET_INPUT;
BIND_SET_PULLUP;
#endif
bool test_bind=IS_BIND_BUTTON_on;
#ifndef STM32_BOARD
if(led)
LED_on;
else
LED_off;
LED_output;
#endif
if( test_bind )
{ // Increase bank
LED_on;
bank++;
if(bank>=NBR_BANKS)
bank=0;
eeprom_write_byte((EE_ADDR)EEPROM_BANK_OFFSET,bank);
debugln("Using bank %d", bank);
phase=3;
blink+=BLINK_BANK_REPEAT;
check+=2*BLINK_BANK_REPEAT;
}
check+=1;
}
}
return bank;
}
#endif
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_off)
{
#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
}
void rf_switch(uint8_t comp)
{
PE1_off;
PE2_off;
switch(comp)
{
case SW_CC2500:
PE2_on;
break;
case SW_CYRF:
PE2_on;
case SW_NRF:
PE1_on;
break;
}
}
// Protocol start
static void protocol_init()
{
if(IS_WAIT_BIND_off)
{
remote_callback = 0; // No protocol
LED_off; // Led off during protocol init
crc16_polynomial = 0x1021; // Default CRC crc16_polynomial
crc8_polynomial = 0x31; // Default CRC crc8_polynomial
prev_option = option;
multi_protocols_index = 0xFF;
// reset telemetry
#ifdef TELEMETRY
#ifdef MULTI_SYNC
inputRefreshRate = 0; // Don't do it unless the protocol asks for it
#endif
tx_pause();
init_frskyd_link_telemetry();
pps_timer=millis();
pps_counter=0;
#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;
tx_resume();
#if defined(AFHDS2A_RX_A7105_INO) || defined(FRSKY_RX_CC2500_INO) || defined(BAYANG_RX_NRF24L01_INO) || defined(DSM_RX_CYRF6936_INO)
for(uint8_t ch=0; ch<16; ch++)
rx_rc_chan[ch] = 1024;
#endif
#endif
binding_idx=0;
//Stop CPPM if it was previously running
#ifdef SEND_CPPM
release_trainer_ppm();
#endif
//Set global ID and rx_tx_addr
MProtocol_id = RX_num + MProtocol_id_master;
set_rx_tx_addr(MProtocol_id);
#ifdef FAILSAFE_ENABLE
FAILSAFE_VALUES_off;
#endif
DATA_BUFFER_LOW_off;
SUB_PROTO_INVALID;
option_override = 0xFF;
blink=millis();
debugln("Protocol selected: %d, sub proto %d, rxnum %d, option %d", protocol, sub_protocol, RX_num, option);
if(protocol)
{
//Reset all modules
modules_reset();
uint8_t index=0;
//#if defined(FRSKYX_CC2500_INO) && defined(MULTI_EU)
// if( ! ( (protocol == PROTO_FRSKYX || protocol == PROTO_FRSKYX2) && sub_protocol < 2 ) )
//#endif
while(multi_protocols[index].protocol != 0xFF)
{
if(multi_protocols[index].protocol==protocol)
{
//Save index
multi_protocols_index = index;
//Check sub protocol validity
if( ((sub_protocol&0x07) == 0) || (sub_protocol&0x07) < multi_protocols[index].nbrSubProto )
SUB_PROTO_VALID;
if(IS_SUB_PROTO_VALID)
{//Start the protocol
//Set the RF switch
rf_switch(multi_protocols[index].rfSwitch);
//Init protocol
multi_protocols[index].Init(); // Init could invalidate the sub proto in case it is not suuported
if(IS_SUB_PROTO_VALID)
remote_callback = multi_protocols[index].CallBack; //Save call back function address
}
#ifdef DEBUG_SERIAL
debug("Proto=%s", multi_protocols[index].ProtoString);
debug(", nbr_sub=%d, Sub=", multi_protocols[index].nbrSubProto);
if(IS_SUB_PROTO_VALID)
{
uint8_t len=multi_protocols[index].SubProtoString[0];
uint8_t offset=len*(sub_protocol&0x07)+1;
for(uint8_t j=0;j<len;j++)
debug("%c",multi_protocols[index].SubProtoString[j+offset]);
}
debug(", Opt=%d",multi_protocols[index].optionType);
debug(", FS=%d",multi_protocols[index].failSafe);
debug(", CHMap=%d",multi_protocols[index].chMap);
debugln(", rfSw=%d",multi_protocols[index].rfSwitch);
#endif
break;
}
index++;
}
//Send a telemetry status right now
SEND_MULTI_STATUS_on;
Update_Telem();
}
#ifdef MULTI_TELEMETRY
else
{//protocol=PROTO_PROTOLIST=0
remote_callback = PROTOLIST_callback;
prev_option = option + 1;
}
#endif
}
#if defined(WAIT_FOR_BIND) && defined(ENABLE_BIND_CH)
if( IS_AUTOBIND_FLAG_on && IS_BIND_CH_PREV_off && (cur_protocol[1]&0x80)==0 && mode_select == MODE_SERIAL)
{ // Autobind is active but no bind requested by either BIND_CH or BIND. But do not wait if in PPM mode...
WAIT_BIND_on;
return;
}
#endif
WAIT_BIND_off;
CHANGE_PROTOCOL_FLAG_off;
if(protocol)
{
//Wait 5ms after protocol init
cli(); // disable global int
OCR1A = TCNT1 + 5000*2; // set compare A for callback
#ifndef STM32_BOARD
TIFR1 = OCF1A_bm ; // clear compare A flag
#else
TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag
#endif
sei(); // enable global int
BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change
}
}
void update_serial_data()
{
static bool prev_ch_mapping=false;
#if defined(TELEMETRY) && defined(INVERT_TELEMETRY_TX)
#ifdef INVERT_TELEMETRY
static bool prev_inv_telem=true;
#else
static bool prev_inv_telem=false;
#endif
#endif
RX_DONOTUPDATE_on;
RX_FLAG_off; //data is being processed
#ifdef SAMSON // Extremely dangerous, do not enable this unless you know what you are doing...
if( rx_ok_buff[0]==0x55 && (rx_ok_buff[1]&0x1F)==PROTO_FRSKYD && rx_ok_buff[2]==0x7F && rx_ok_buff[24]==217 && rx_ok_buff[25]==202 )
{//proto==FRSKYD+sub==7+rx_num==7+CH15==73%+CH16==73%
rx_ok_buff[1]=(rx_ok_buff[1]&0xE0) | PROTO_FLYSKY; // change the protocol to Flysky
memcpy((void*)(rx_ok_buff+4),(void*)(rx_ok_buff+4+11),11); // reassign channels 9-16 to 1-8
}
#endif
#ifdef BONI // Extremely dangerous, do not enable this!!! This is really for a special case...
if(CH14_SW)
rx_ok_buff[2]=(rx_ok_buff[2]&0xF0)|((rx_ok_buff[2]+1)&0x0F);
#endif
if(rx_ok_buff[1]&0x20) //check range
RANGE_FLAG_on;
else
RANGE_FLAG_off;
if(rx_ok_buff[1]&0x40) //check autobind
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
//Forced frequency tuning values for CC2500 protocols
#if defined(FORCE_FRSKYD_TUNING) && defined(FRSKYD_CC2500_INO)
if(protocol==PROTO_FRSKYD)
option=FORCE_FRSKYD_TUNING; // Use config-defined tuning value for FrSkyD
else
#endif
#if defined(FORCE_FRSKYL_TUNING) && defined(FRSKYL_CC2500_INO)
if(protocol==PROTO_FRSKYL)
option=FORCE_FRSKYL_TUNING; // Use config-defined tuning value for FrSkyL
else
#endif
#if defined(FORCE_FRSKYV_TUNING) && defined(FRSKYV_CC2500_INO)
if(protocol==PROTO_FRSKYV)
option=FORCE_FRSKYV_TUNING; // Use config-defined tuning value for FrSkyV
else
#endif
#if defined(FORCE_FRSKYX_TUNING) && defined(FRSKYX_CC2500_INO)
if(protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2)
option=FORCE_FRSKYX_TUNING; // Use config-defined tuning value for FrSkyX
else
#endif
#if defined(FORCE_FUTABA_TUNING) && defined(FUTABA_CC2500_INO)
if (protocol==PROTO_FUTABA)
option=FORCE_FUTABA_TUNING; // Use config-defined tuning value for SFHSS
else
#endif
#if defined(FORCE_CORONA_TUNING) && defined(CORONA_CC2500_INO)
if (protocol==PROTO_CORONA)
option=FORCE_CORONA_TUNING; // Use config-defined tuning value for CORONA
else
#endif
#if defined(FORCE_SKYARTEC_TUNING) && defined(SKYARTEC_CC2500_INO)
if (protocol==PROTO_SKYARTEC)
option=FORCE_SKYARTEC_TUNING; // Use config-defined tuning value for SKYARTEC
else
#endif
#if defined(FORCE_REDPINE_TUNING) && defined(REDPINE_CC2500_INO)
if (protocol==PROTO_REDPINE)
option=FORCE_REDPINE_TUNING; // Use config-defined tuning value for REDPINE
else
#endif
#if defined(FORCE_RADIOLINK_TUNING) && defined(RADIOLINK_CC2500_INO)
if (protocol==PROTO_RADIOLINK)
option = FORCE_RADIOLINK_TUNING; // Use config-defined tuning value for RADIOLINK
else
#endif
#if defined(FORCE_HITEC_TUNING) && defined(HITEC_CC2500_INO)
if (protocol==PROTO_HITEC)
option=FORCE_HITEC_TUNING; // Use config-defined tuning value for HITEC
else
#endif
#if defined(FORCE_HOTT_TUNING) && defined(HOTT_CC2500_INO)
if (protocol==PROTO_HOTT)
option=FORCE_HOTT_TUNING; // Use config-defined tuning value for HOTT
else
#endif
option=rx_ok_buff[3]; // Use radio-defined option value
#ifdef FAILSAFE_ENABLE
bool failsafe=false;
if(rx_ok_buff[0]&0x02)
{ // Packet contains failsafe instead of channels
failsafe=true;
rx_ok_buff[0]&=0xFD; // Remove the failsafe flag
FAILSAFE_VALUES_on; // Failsafe data has been received
debugln("Failsafe received");
}
#endif
DISABLE_CH_MAP_off;
DISABLE_TELEM_off;
if(rx_len>26)
{//Additional flag received at the end
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]
if(rx_ok_buff[26]&0x02)
DISABLE_TELEM_on;
if(rx_ok_buff[26]&0x01)
DISABLE_CH_MAP_on;
#if defined(TELEMETRY) && defined(INVERT_TELEMETRY_TX)
if(((rx_ok_buff[26]&0x08)!=0) ^ prev_inv_telem)
{ //value changed
if(rx_ok_buff[26]&0x08)
{ // Invert telemetry
debugln("Invert telem %d,%d",rx_ok_buff[26]&0x01,prev_inv_telem);
#if defined (ORANGE_TX)
PORTC.PIN3CTRL |= 0x40 ;
#elif defined (STM32_BOARD)
TX_INV_on;
RX_INV_on;
#endif
}
else
{ // Normal telemetry
debugln("Normal telem %d,%d",rx_ok_buff[26]&0x01,prev_inv_telem);
#if defined (ORANGE_TX)
PORTC.PIN3CTRL &= 0xBF ;
#elif defined (STM32_BOARD)
TX_INV_off;
RX_INV_off;
#endif
}
prev_inv_telem=rx_ok_buff[26]&0x08;
}
#endif
}
if( (rx_ok_buff[0] != cur_protocol[0]) || ((rx_ok_buff[1]&0x5F) != (cur_protocol[1]&0x5F)) || ( (rx_ok_buff[2]&0x7F) != (cur_protocol[2]&0x7F) ) )
{ // New model has been selected
CHANGE_PROTOCOL_FLAG_on; //change protocol
WAIT_BIND_off;
if((rx_ok_buff[1]&0x80)!=0 || IS_AUTOBIND_FLAG_on)
BIND_IN_PROGRESS; //launch bind right away if in autobind mode or bind is set
else
BIND_DONE;
protocol=rx_ok_buff[1]&0x1F; //protocol no (0-31)
if(!(rx_ok_buff[0]&1))
protocol+=32; //protocol no (0-63)
if(rx_len>26)
protocol|=rx_ok_buff[26]&0xC0; //protocol no (0-255)
sub_protocol=(rx_ok_buff[2]>>4)& 0x07; //subprotocol no (0-7) bits 4-6
RX_num=rx_ok_buff[2]& 0x0F; //rx_num no (0-15)
if(rx_len>26)
RX_num|=rx_ok_buff[26]&0x30; //rx_num no (0-63)
}
else
if( ((rx_ok_buff[1]&0x80)!=0) && ((cur_protocol[1]&0x80)==0) ) // Bind flag has been set
{ // Restart protocol with bind
CHANGE_PROTOCOL_FLAG_on;
BIND_IN_PROGRESS;
}
else
if( ((rx_ok_buff[1]&0x80)==0) && ((cur_protocol[1]&0x80)!=0) ) // Bind flag has been reset
{ // Request protocol to end bind
End_Bind();
}
//store current protocol values
for(uint8_t i=0;i<3;i++)
cur_protocol[i] = rx_ok_buff[i];
//disable channel mapping
if(multi_protocols[multi_protocols_index].chMap == 0)
DISABLE_CH_MAP_off; //not a protocol supporting ch map to be disabled
if(prev_ch_mapping!=IS_DISABLE_CH_MAP_on)
{
prev_ch_mapping=IS_DISABLE_CH_MAP_on;
if(IS_DISABLE_CH_MAP_on)
{
for(uint8_t i=0;i<4;i++)
CH_AETR[i]=CH_TAER[i]=CH_EATR[i]=i;
debugln("DISABLE_CH_MAP_on");
}
else
{
CH_AETR[0]=AILERON;CH_AETR[1]=ELEVATOR;CH_AETR[2]=THROTTLE;CH_AETR[3]=RUDDER;
CH_TAER[0]=THROTTLE;CH_TAER[1]=AILERON;CH_TAER[2]=ELEVATOR;CH_TAER[3]=RUDDER;
CH_EATR[0]=ELEVATOR;CH_EATR[1]=AILERON;CH_EATR[2]=THROTTLE;CH_EATR[3]=RUDDER;
debugln("DISABLE_CH_MAP_off");
}
}
// decode channel/failsafe values
volatile uint8_t *p=rx_ok_buff+3;
uint8_t dec=-3;
for(uint8_t i=0;i<NUM_CHN;i++)
{
dec+=3;
if(dec>=8)
{
dec-=8;
p++;
}
p++;
uint16_t temp=((*((uint32_t *)p))>>dec)&0x7FF;
#ifdef FAILSAFE_ENABLE
if(failsafe)
Failsafe_data[i]=temp; //value range 0..2047, 0=no pulse, 2047=hold
else
#endif
Channel_data[i]=temp; //value range 0..2047, 0=-125%, 2047=+125%
}
#ifdef HOTT_FW_TELEMETRY
HoTT_SerialRX=false;
#endif
if(rx_len>27)
{ // Data available for the current protocol
#if defined(FRSKYX_CC2500_INO) || defined(FRSKYR9_SX1276_INO)
if((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || protocol==PROTO_FRSKY_R9) && rx_len==28)
{//Protocol waiting for 1 byte during bind
binding_idx=rx_ok_buff[27];
}
#endif
#ifdef SPORT_SEND
if((protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2 || protocol==PROTO_FRSKY_R9) && rx_len==27+8)
{//Protocol waiting for 8 bytes
#define BYTE_STUFF 0x7D
#define STUFF_MASK 0x20
//debug("SPort_in: ");
boolean sport_valid=false;
for(uint8_t i=28;i<28+7;i++)
if(rx_ok_buff[i]!=0) sport_valid=true; //Check that the payload is not full of 0
if((rx_ok_buff[27]&0x1F) > 0x1B) //Check 1st byte validity
sport_valid=false;
if(sport_valid)
{
SportData[SportTail]=0x7E;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
SportData[SportTail]=rx_ok_buff[27]&0x1F;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
for(uint8_t i=28;i<28+7;i++)
{
if( (rx_ok_buff[i]==BYTE_STUFF) || (rx_ok_buff[i]==0x7E) )
{//stuff
SportData[SportTail]=BYTE_STUFF;
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
SportData[SportTail]=rx_ok_buff[i]^STUFF_MASK;
}
else
SportData[SportTail]=rx_ok_buff[i];
//debug("%02X ",SportData[SportTail]);
SportTail = (SportTail+1) & (MAX_SPORT_BUFFER-1);
}
uint8_t used = SportTail;
if ( SportHead > SportTail )
used += MAX_SPORT_BUFFER - SportHead ;
else
used -= SportHead ;
if ( used >= MAX_SPORT_BUFFER-(MAX_SPORT_BUFFER>>2) )
{
DATA_BUFFER_LOW_on;
//Send Multi Status ASAP to inform the TX
SEND_MULTI_STATUS_on;
Update_Telem();
debugln("Low buf=%d,h=%d,t=%d",used,SportHead,SportTail);
}
}
}
#endif //SPORT_SEND
#ifdef HOTT_FW_TELEMETRY
if(protocol==PROTO_HOTT && rx_len==27+1)
{//Protocol waiting for 1 byte
HoTT_SerialRX_val=rx_ok_buff[27];
HoTT_SerialRX=true;
}
#endif
#ifdef DSM_FWD_PGM
if(protocol==PROTO_DSM && rx_len==27+7)
{//Protocol waiting for 7 bytes
memcpy(DSM_SerialRX_val, (const void *)&rx_ok_buff[27],7);
DSM_SerialRX=true;
}
#endif
#ifdef MULTI_CONFIG_INO
if(protocol==PROTO_CONFIG && rx_len==27+7)
{//Protocol waiting for 7 bytes
memcpy(CONFIG_SerialRX_val, (const void *)&rx_ok_buff[27],7);
CONFIG_SerialRX=true;
}
#endif
}
RX_DONOTUPDATE_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
{
if(rx_idx>=26 && rx_idx<RXBUFFER_SIZE)
{
rx_len=rx_idx;
memcpy((void*)rx_ok_buff,(const void*)rx_buff,rx_len);// 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
#ifdef SX1276_INSTALLED
SX1276_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;
}
USART2_BASE->CR1 &= ~ USART_CR1_TE; //disable transmit
usart3_begin(100000,SERIAL_8E2);
#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_PIN
#if defined(TELEMETRY)
initTXSerial( SPEED_100K ) ;
#endif //TELEMETRY
#endif //DEBUG_PIN
#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 STM32_BOARD
void usart2_begin(uint32_t baud,uint32_t config )
{
usart_init(USART2);
usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config);
LED2_output;
usart_set_baud_rate(USART2, STM32_PCLK1, baud);
usart_enable(USART2);
}
void usart3_begin(uint32_t baud,uint32_t config )
{
usart_init(USART3);
usart_config_gpios_async(USART3,GPIOB,PIN_MAP[PB11].gpio_bit,GPIOB,PIN_MAP[PB10].gpio_bit,config);
usart_set_baud_rate(USART3, STM32_PCLK1, baud);
USART3_BASE->CR3 &= ~USART_CR3_EIE & ~USART_CR3_CTSIE; // Disable receive
USART3_BASE->CR1 &= ~USART_CR1_RE & ~USART_CR1_RXNEIE & ~USART_CR1_PEIE & ~USART_CR1_IDLEIE ; // Disable RX and interrupts
USART3_BASE->CR1 |= (USART_CR1_TE | USART_CR1_UE); // Enable USART3 and TX
}
void init_HWTimer()
{
HWTimer2.pause(); // Pause the timer2 while we're configuring it
TIMER2_BASE->PSC = 35; // 36-1;for 72 MHZ /0.5sec/(35+1)
TIMER2_BASE->ARR = 0xFFFF; // Count until 0xFFFF
HWTimer2.setMode(TIMER_CH1, TIMER_OUTPUT_COMPARE); // Main scheduler
TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer2/Comp2 interrupt flag
TIMER2_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer2/Comp2 interrupt
HWTimer2.refresh(); // Refresh the timer's count, prescale, and overflow
HWTimer2.resume();
#ifdef ENABLE_SERIAL
HWTimer3.pause(); // Pause the timer3 while we're configuring it
TIMER3_BASE->PSC = 35; // 36-1;for 72 MHZ /0.5sec/(35+1)
TIMER3_BASE->ARR = 0xFFFF; // Count until 0xFFFF
HWTimer3.setMode(TIMER_CH2, TIMER_OUTPUT_COMPARE); // Serial check
TIMER3_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer3/Comp2 interrupt flag
HWTimer3.attachInterrupt(TIMER_CH2,ISR_COMPB); // Assign function to Timer3/Comp2 interrupt
TIMER3_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer3/Comp2 interrupt
HWTimer3.refresh(); // Refresh the timer's count, prescale, and overflow
HWTimer3.resume();
#endif
}
#endif
#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
nvic_sys_reset();
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==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)
#ifdef TELEMETRY_FRSKYX_TO_FRSKYD
|| (protocol==PROTO_FRSKYX) || (protocol==PROTO_FRSKYX2)
#endif
)
initTXSerial( SPEED_9600 ) ;
#ifndef TELEMETRY_FRSKYX_TO_FRSKYD
if(protocol==PROTO_FRSKYX || protocol==PROTO_FRSKYX2)
initTXSerial( SPEED_57600 ) ;
#endif
if(protocol==PROTO_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);
}
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
{
//debugln("Read ID from EEPROM");
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];
debugln("Generated ID from STM32 UUID");
}
else
#endif
id = random(0xfefefefe) + ((uint32_t)random(0xfefefefe) << 16);
for(uint8_t i=0;i<4;i++)
eeprom_write_byte((EE_ADDR)address+i,id >> (i*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
}
// Generate frequency hopping sequence in the range [02..77]
static void __attribute__((unused)) calc_fh_channels(uint8_t num_ch)
{
uint8_t idx = 0;
uint32_t rnd = MProtocol_id;
uint8_t max=(num_ch/3)+2;
while (idx < num_ch)
{
uint8_t i;
uint8_t count_2_26 = 0, count_27_50 = 0, count_51_74 = 0;
rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization
// Use least-significant byte. 73 is prime, so channels 76..77 are unused
uint8_t next_ch = ((rnd >> 8) % 73) + 2;
// Keep a distance of 5 between consecutive channels
if (idx !=0)
{
if(hopping_frequency[idx-1]>next_ch)
{
if(hopping_frequency[idx-1]-next_ch<5)
continue;
}
else
if(next_ch-hopping_frequency[idx-1]<5)
continue;
}
// Check that it's not duplicated and spread uniformly
for (i = 0; i < idx; i++) {
if(hopping_frequency[i] == next_ch)
break;
if(hopping_frequency[i] <= 26)
count_2_26++;
else if (hopping_frequency[i] <= 50)
count_27_50++;
else
count_51_74++;
}
if (i != idx)
continue;
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) )
hopping_frequency[idx++] = next_ch;//find hopping frequency
}
}
static uint8_t __attribute__((unused)) bit_reverse(uint8_t b_in)
{
uint8_t b_out = 0;
for (uint8_t i = 0; i < 8; ++i)
{
b_out = (b_out << 1) | (b_in & 1);
b_in >>= 1;
}
return b_out;
}
static void __attribute__((unused)) crc16_update(uint8_t a, uint8_t bits)
{
crc ^= a << 8;
while(bits--)
if (crc & 0x8000)
crc = (crc << 1) ^ crc16_polynomial;
else
crc = crc << 1;
}
static void __attribute__((unused)) crc8_update(uint8_t byte)
{
crc8 = crc8 ^ byte;
for ( uint8_t j = 0; j < 8; j++ )
if ( crc8 & 0x80 )
crc8 = (crc8<<1) ^ crc8_polynomial;
else
crc8 <<= 1;
}
/**************************/
/**************************/
/** 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<1600)
bad_frame=1; // bad frame
else
if(Cur_TCNT1>4400)
{ //start of frame
if(chan>=MIN_PPM_CHANNELS)
{
PPM_FLAG_on; // good frame received if at least 4 channels have been seen
if(chan>PPM_chan_max) PPM_chan_max=chan; // Saving the number of channels received
}
chan=0; // reset channel counter
bad_frame=0;
}
else
if(bad_frame==0) // need to wait for start of frame
{ //servo values between 800us and 2200us will end up here
PPM_data[chan]=Cur_TCNT1;
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
#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(rx_idx==0||discard_frame==true)
{ // Let's try to sync at this point
RX_MISSED_BUFF_off; // If rx_buff was good it's not anymore...
rx_idx=0;discard_frame=false;
rx_buff[0]=UDR0;
#ifdef FAILSAFE_ENABLE
if((rx_buff[0]&0xFC)==0x54) // If 1st byte is 0x54, 0x55, 0x56 or 0x57 it looks ok
#else
if((rx_buff[0]&0xFE)==0x54) // If 1st byte is 0x54 or 0x55 it looks ok
#endif
{
#if defined STM32_BOARD
TIMER3_BASE->CCR2=TIMER3_BASE->CNT + 500; // Next byte should show up within 250us (1 byte = 120us)
TIMER3_BASE->SR = 0x1E5F & ~TIMER_SR_CC2IF; // Clear Timer3/Comp2 interrupt flag
TIMER3_BASE->DIER |= TIMER_DIER_CC2IE; // Enable Timer3/Comp2 interrupt
#else
TX_RX_PAUSE_on;
tx_pause();
cli(); // Disable global int due to RW of 16 bits registers
OCR1B = TCNT1 + 500; // Next byte should show up within 250us (1 byte = 120us)
sei(); // Enable global int
TIFR1 = OCF1B_bm ; // clear OCR1B match flag
SET_TIMSK1_OCIE1B ; // enable interrupt on compare B match
#endif
rx_idx++;
}
}
else
{
if(rx_idx>=RXBUFFER_SIZE)
{
discard_frame=true; // Too many bytes being received...
debugln("RX frame too long");
}
else
{
rx_buff[rx_idx++]=UDR0; // Store received byte
#if defined STM32_BOARD
TIMER3_BASE->CCR2=TIMER3_BASE->CNT + 500; // Next byte should show up within 250us (1 byte = 120us)
#else
cli(); // Disable global int due to RW of 16 bits registers
OCR1B = TCNT1 + 500; // Next byte should show up within 250us (1 byte = 120us)
sei(); // Enable global int
#endif
}
}
}
else
{
rx_idx=UDR0; // Dummy read
rx_idx=0;
discard_frame=true; // Error encountered discard full frame...
debugln("Bad frame RX");
}
if(discard_frame==true)
{
#ifdef STM32_BOARD
TIMER3_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer3/Comp2 interrupt
#else
CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
TX_RX_PAUSE_off;
tx_resume();
#endif
}
#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)
#endif
{ // Timer1 compare B interrupt
if(rx_idx>=26 && rx_idx<=RXBUFFER_SIZE)
{
// A full frame has been received
if(!IS_RX_DONOTUPDATE_on)
{ //Good frame received and main is not working on the buffer
rx_len=rx_idx;
memcpy((void*)rx_ok_buff,(const void*)rx_buff,rx_idx); // Duplicate the buffer
RX_FLAG_on; // Flag for main to process data
}
else
RX_MISSED_BUFF_on; // Notify that rx_buff is good
#ifdef MULTI_SYNC
cli();
last_serial_input=TCNT1;
sei();
#endif
}
#ifdef DEBUG_SERIAL
else
debugln("RX frame size incorrect");
#endif
discard_frame=true;
#ifdef STM32_BOARD
TIMER3_BASE->DIER &= ~TIMER_DIER_CC2IE; // Disable Timer3/Comp2 interrupt
#else
CLR_TIMSK1_OCIE1B; // Disable interrupt on compare B match
TX_RX_PAUSE_off;
tx_resume();
#endif
}
#endif //ENABLE_SERIAL
/**************************/
/**************************/
/** CPPM routines **/
/**************************/
/**************************/
#ifdef SEND_CPPM
#define PPM_CENTER 1500*2
uint32_t TrainerTimer ;
bool CppmInitialised = false;
uint16_t *TrainerPulsePtr ;
uint16_t TrainerPpmStream[10] ;
int16_t CppmChannels[8] ;
void setupTrainerPulses()
{
uint32_t i ;
uint32_t total ;
uint32_t pulse ;
uint16_t *ptr ;
uint32_t p = 8 ;
int16_t PPM_range = 512*2 ; //range of 0.7..1.7msec
ptr = TrainerPpmStream ;
total = 22500u*2; //Minimum Framelen=22.5 ms
if ( (millis() - TrainerTimer) < 400 )
{
for ( i = 0 ; i < p ; i += 1 )
{
pulse = max( (int)min(CppmChannels[i],PPM_range),-PPM_range) + PPM_CENTER ;
total -= pulse ;
*ptr++ = pulse ;
}
}
*ptr++ = total ;
*ptr = 0 ;
TIMER1_BASE->CCR1 = total - 1500 ; // Update time
TIMER1_BASE->CCR2 = 300*2 ;
}
void init_trainer_ppm()
{
// Timer 1, channel 2 on PA9
RCC_BASE->APB2ENR |= RCC_APB2ENR_TIM1EN ; // Enable clock
setupTrainerPulses() ;
RCC_BASE->APB2ENR |= RCC_APB2ENR_IOPAEN ; // Enable portA clock
RCC_BASE->APB2ENR &= ~RCC_APB2ENR_USART1EN ; // Disable USART1
GPIOA_BASE->CRH &= ~0x00F0 ;
GPIOA_BASE->CRH |= 0x00A0 ; // AF PP OP2MHz
HWTimer1.pause() ; // Pause the timer1 while we're configuring it
TIMER1_BASE->ARR = *TrainerPulsePtr++ ;
TIMER1_BASE->PSC = 72000000 / 2000000 - 1 ; // 0.5uS
TIMER1_BASE->CCR2 = 600 ; // 300 uS pulse
TIMER1_BASE->CCR1 = 5000 ; // 2500 uS pulse
TIMER1_BASE->CCMR1 = 0x6000 ; // PWM mode 1 (header file has incorrect bits)
TIMER1_BASE->EGR = 1 ;
TIMER1_BASE->CCER = TIMER_CCER_CC2E ;
TIMER1_BASE->DIER |= TIMER_DIER_UIE ;
TIMER1_BASE->CR1 = TIMER_CR1_CEN ;
nvic_irq_set_priority(NVIC_TIMER1_CC, 4 ) ;
nvic_irq_set_priority(NVIC_TIMER1_UP, 4 ) ;
HWTimer1.attachInterrupt(TIMER_UPDATE_INTERRUPT,tim1_up); // Assign function to Timer1/Comp2 interrupt
HWTimer1.attachInterrupt(TIMER_CH1,tim1_cc); // Assign function to Timer1/Comp2 interrupt
CppmInitialised = true ;
HWTimer1.resume() ;
}
void release_trainer_ppm()
{
if ( CppmInitialised )
{
TIMER1_BASE->CR1 = 0 ;
pinMode(PA9,INPUT) ;
CppmInitialised = false ;
}
}
void tim1_up()
{
#define TIMER1_SR_MASK 0x1FFF
// PPM out update interrupt
if ( (TIMER1_BASE->DIER & TIMER_DIER_UIE) && ( TIMER1_BASE->SR & TIMER_SR_UIF ) )
{
GPIOA_BASE->BRR = 0x0200 ;
TIMER1_BASE->SR = TIMER1_SR_MASK & ~TIMER_SR_UIF ; // Clear flag
TIMER1_BASE->ARR = *TrainerPulsePtr++ ;
if ( *TrainerPulsePtr == 0 )
{
TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_CC1IF ; // Clear this flag
TIMER1_BASE->DIER |= TIMER_DIER_CC1IE ; // Enable this interrupt
TIMER1_BASE->DIER &= ~TIMER_DIER_UIE ; // Stop this interrupt
}
}
}
void tim1_cc()
{
if ( ( TIMER1_BASE->DIER & TIMER_DIER_CC1IE ) && ( TIMER1_BASE->SR & TIMER_SR_CC1IF ) )
{
// compare interrupt
TIMER1_BASE->DIER &= ~TIMER_DIER_CC1IE ; // Stop this interrupt
TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_CC1IF ; // Clear flag
setupTrainerPulses() ;
TrainerPulsePtr = TrainerPpmStream ;
TIMER1_BASE->SR = 0x1FFF & ~TIMER_SR_UIF ; // Clear this flag
TIMER1_BASE->DIER |= TIMER_DIER_UIE ; // Enable this interrupt
}
}
void Send_CCPM_USART1()
{
if ( CppmInitialised == false )
init_trainer_ppm() ;
TrainerTimer = millis() ;
len = packet_in[3] ;
uint32_t bitsavailable = 0 ;
uint32_t bits = 0 ; ;
uint32_t i ;
int16_t value ;
uint8_t *packet ;
packet = &packet_in[4] ;
i = packet_in[2] ; // Start channel
// Load changed channels
while ( len )
{
while ( bitsavailable < 11 )
{
bits |= *packet++ << bitsavailable ;
bitsavailable += 8 ;
}
value = bits & 0x07FF ;
value -= 0x0400 ;
bitsavailable -= 11 ;
bits >>= 11 ;
if ( i < 8 )
CppmChannels[i] = value * 5 / 4 ;
i++ ;
len-- ;
}
}
#endif
/**************************/
/**************************/
/** Arduino random **/
/**************************/
/**************************/
#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;
}
// 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