/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see .
*/
//**************************
// Telemetry serial code *
//**************************
#if defined TELEMETRY
#if defined MULTI_TELEMETRY
#define MULTI_TIME 1000*1000*1000 // 1s
uint32_t lastMulti = 0;
#endif
#if defined SPORT_TELEMETRY
#define SPORT_TIME 12000 //12ms
#define FRSKY_SPORT_PACKET_SIZE 8
uint32_t last = 0;
uint8_t sport_counter=0;
uint8_t RxBt = 0;
uint8_t rssi;
uint8_t sport = 0;
#endif
#if defined HUB_TELEMETRY
#define USER_MAX_BYTES 6
uint8_t prev_index;
#endif
#define START_STOP 0x7e
#define BYTESTUFF 0x7d
#define STUFF_MASK 0x20
#define MAX_PKTX 10
uint8_t pktx[MAX_PKTX];
uint8_t pktx1[MAX_PKTX];
uint8_t indx;
uint8_t frame[18];
#ifdef BASH_SERIAL
// For bit-bashed serial output
volatile struct t_serial_bash
{
uint8_t head ;
uint8_t tail ;
uint8_t data[64] ;
uint8_t busy ;
uint8_t speed ;
} SerialControl ;
#endif
#ifdef DSM_TELEMETRY
void DSM_frame()
{
Serial_write(0xAA); // Telemetry packet
for (uint8_t i = 0; i < 17; i++) // RSSI value followed by 16 bytes of telemetry data
Serial_write(pkt[i]);
}
#endif
#ifdef AFHDS2A_FW_TELEMETRY
void AFHDSA_short_frame()
{
Serial_write(0xAA); // Telemetry packet
for (uint8_t i = 0; i < 29; i++) // RSSI value followed by 4*7 bytes of telemetry data
Serial_write(pkt[i]);
}
#endif
void frskySendStuffed()
{
Serial_write(START_STOP);
for (uint8_t i = 0; i < 9; i++)
{
if ((frame[i] == START_STOP) || (frame[i] == BYTESTUFF))
{
Serial_write(BYTESTUFF);
frame[i] ^= STUFF_MASK;
}
Serial_write(frame[i]);
}
Serial_write(START_STOP);
}
void compute_RSSIdbm()
{
RSSI_dBm = (((uint16_t)(pktt[len-2])*18)>>4);
if(pktt[len-2] >=128)
RSSI_dBm -= 164;
else
RSSI_dBm += 130;
}
void frsky_check_telemetry(uint8_t *pkt,uint8_t len)
{
if(pkt[1] == rx_tx_addr[3] && pkt[2] == rx_tx_addr[2] && len ==(pkt[0] + 3))
{
for (uint8_t i=3;i> 4 & 0x0f) == 0x08)
{
seq_last_sent = 8;
seq_last_rcvd = 0;
pass=0;
}
else
{
if ((pktt[5] >> 4 & 0x03) == (seq_last_rcvd + 1) % 4)
seq_last_rcvd = (seq_last_rcvd + 1) % 4;
else
pass=0;//reset if sequence wrong
}
}
#endif
}
}
void init_hub_telemetry()
{
telemetry_link=0;
telemetry_counter=0;
v_lipo1=0;
v_lipo2=0;
RSSI_dBm=0;
TX_RSSI=0;
}
void frsky_link_frame()
{
frame[0] = 0xFE;
if (protocol==MODE_FRSKYD)
{
compute_RSSIdbm();
frame[1] = pktt[3];
frame[2] = pktt[4];
frame[3] = pktt[5];
frame[4] = (uint8_t)RSSI_dBm;
}
else
if (protocol==MODE_HUBSAN||protocol==MODE_AFHDS2A||protocol==MODE_BAYANG)
{
frame[1] = v_lipo1;
frame[2] = v_lipo2;
frame[3] = (uint8_t)RSSI_dBm;
frame[4] = TX_RSSI;
}
frame[5] = frame[6] = frame[7] = frame[8] = 0;
frskySendStuffed();
}
#if defined HUB_TELEMETRY
void frsky_user_frame()
{
uint8_t indexx = 0, j=8, i;
//uint8_t c=0, n=0;
if(pktt[6]>0 && pktt[6]<=10)
{//only valid hub frames
frame[0] = 0xFD;
frame[2] = pktt[7];
switch(pass)
{
case 0:
indexx=pktt[6];
for(i=0;i0)
{
crc_s += p[i]; //0-1FF
crc_s += crc_s >> 8; //0-100
crc_s &= 0x00ff;
}
}
}
void sportIdle()
{
Serial_write(START_STOP);
}
void sportSendFrame()
{
uint8_t i;
sport_counter = (sport_counter + 1) %36;
if(telemetry_lost)
{
sportIdle();
return;
}
if(sport_counter<6)
{
frame[0] = 0x98;
frame[1] = 0x10;
for (i=5;i<8;i++)
frame[i]=0;
}
switch (sport_counter)
{
case 0:
frame[2] = 0x05;
frame[3] = 0xf1;
frame[4] = 0x02 ;//dummy values if swr 20230f00
frame[5] = 0x23;
frame[6] = 0x0F;
break;
case 2: // RSSI
frame[2] = 0x01;
frame[3] = 0xf1;
frame[4] = rssi;
break;
case 4: //BATT
frame[2] = 0x04;
frame[3] = 0xf1;
frame[4] = RxBt;//a1;
break;
default:
if(sport)
{
for (i=0;i= FRSKY_SPORT_PACKET_SIZE)
{//8 bytes no crc
if ( sport )
{
// overrun!
}
else
{
uint8_t i ;
for ( i = 0 ; i < FRSKY_SPORT_PACKET_SIZE ; i += 1 )
{
pktx1[i] = pktx[i] ; // Double buffer
}
sport = 1;//ok to send
}
pass = 0;//reset
}
}
#endif
void TelemetryUpdate()
{
// check for space in tx buffer
#ifdef BASH_SERIAL
uint8_t h ;
uint8_t t ;
h = SerialControl.head ;
t = SerialControl.tail ;
if ( h >= t )
{
t += 64 - h ;
}
else
{
t -= h ;
}
if ( t < 32 )
{
return ;
}
#else
uint8_t h ;
uint8_t t ;
h = tx_head ;
t = tx_tail ;
if ( h >= t )
{
t += TXBUFFER_SIZE - h ;
}
else
{
t -= h ;
}
if ( t < 16 )
{
return ;
}
#endif
#if defined SPORT_TELEMETRY
if (protocol==MODE_FRSKYX)
{ // FrSkyX
if(telemetry_link)
{
if(pktt[4] & 0x80)
rssi=pktt[4] & 0x7F ;
else
RxBt = (pktt[4]<<1) + 1 ;
if(pktt[6]<=6)
for (uint8_t i=0; i < pktt[6]; i++)
proces_sport_data(pktt[7+i]);
telemetry_link=0;
}
uint32_t now = micros();
if ((now - last) > SPORT_TIME)
{
sportSendFrame();
#ifdef STM32_BOARD
last=now;
#else
last += SPORT_TIME ;
#endif
}
}
#endif
#if defined DSM_TELEMETRY
if(telemetry_link && protocol == MODE_DSM )
{ // DSM
DSM_frame();
telemetry_link=0;
return;
}
#endif
#if defined AFHDS2A_FW_TELEMETRY
if(telemetry_link == 2 && protocol == MODE_AFHDS2A)
{
AFHDSA_short_frame();
telemetry_link=0;
}
#endif
if(telemetry_link && protocol != MODE_FRSKYX )
{ // FrSkyD + Hubsan + AFHDS2A + Bayang
frsky_link_frame();
telemetry_link=0;
return;
}
#if defined HUB_TELEMETRY
if(!telemetry_link && protocol == MODE_FRSKYD)
{ // FrSky
frsky_user_frame();
return;
}
#endif
#if defined MULTI_TELEMETRY
{
uint32_t now = micros();
if ((now - lastMulti) > MULTI_TIME) {
sendMultiStatus();
}
}
}
/**************************/
/**************************/
/** Serial TX routines **/
/**************************/
/**************************/
#ifndef BASH_SERIAL
// Routines for normal serial output
void Serial_write(uint8_t data)
{
uint8_t nextHead ;
nextHead = tx_head + 1 ;
if ( nextHead >= TXBUFFER_SIZE )
nextHead = 0 ;
tx_buff[nextHead]=data;
tx_head = nextHead ;
tx_resume();
}
void initTXSerial( uint8_t speed)
{
#ifdef ENABLE_PPM
if(speed==SPEED_9600)
{ // 9600
#ifdef ORANGE_TX
USARTC0.BAUDCTRLA = 207 ;
USARTC0.BAUDCTRLB = 0 ;
USARTC0.CTRLB = 0x18 ;
USARTC0.CTRLA = (USARTC0.CTRLA & 0xCF) | 0x10 ;
USARTC0.CTRLC = 0x03 ;
#else
#ifdef STM32_BOARD
usart3_begin(9600,SERIAL_8N1); //USART3
USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
#else
UBRR0H = 0x00;
UBRR0L = 0x67;
UCSR0A = 0 ; // Clear X2 bit
//Set frame format to 8 data bits, none, 1 stop bit
UCSR0C = (1<CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
#else
UBRR0H = 0x00;
UBRR0L = 0x22;
UCSR0A = 0x02 ; // Set X2 bit
//Set frame format to 8 data bits, none, 1 stop bit
UCSR0C = (1<CR1 &= ~ USART_CR1_RE; //disable RX leave TX enabled
#else
UBRR0H = 0x00;
UBRR0L = 0x07;
UCSR0A = 0x00 ; // Clear X2 bit
//Set frame format to 8 data bits, none, 1 stop bit
UCSR0C = (1<SR & USART_SR_TXE)
{
#endif
if(tx_head!=tx_tail)
{
if(++tx_tail>=TXBUFFER_SIZE)//head
tx_tail=0;
#ifdef STM32_BOARD
USART3_BASE->DR=tx_buff[tx_tail];//clears TXE bit
#else
UDR0=tx_buff[tx_tail];
#endif
}
if (tx_tail == tx_head)
tx_pause(); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
#ifdef STM32_BOARD
}
#endif
}
#ifdef STM32_BOARD
void usart2_begin(uint32_t baud,uint32_t config )
{
usart_init(USART2);
usart_config_gpios_async(USART2,GPIOA,PIN_MAP[PA3].gpio_bit,GPIOA,PIN_MAP[PA2].gpio_bit,config);
usart_set_baud_rate(USART2, STM32_PCLK1, baud);//
usart_enable(USART2);
}
void usart3_begin(uint32_t baud,uint32_t config )
{
usart_init(USART3);
usart_config_gpios_async(USART3,GPIOB,PIN_MAP[PB11].gpio_bit,GPIOB,PIN_MAP[PB10].gpio_bit,config);
usart_set_baud_rate(USART3, STM32_PCLK1, baud);
usart_enable(USART3);
}
#endif
#else //BASH_SERIAL
// Routines for bit-bashed serial output
// Speed is 0 for 100K and 1 for 9600
void initTXSerial( uint8_t speed)
{
TIMSK0 = 0 ; // Stop all timer 0 interrupts
#ifdef INVERT_SERIAL
SERIAL_TX_off;
#else
SERIAL_TX_on;
#endif
UCSR0B &= ~(1<>= 7 ; // Top bit
if ( SerialControl.speed == SPEED_100K )
{
#ifdef INVERT_SERIAL
byteLo |= 0x02 ; // Parity bit
#else
byteLo |= 0xFC ; // Stop bits
#endif
// calc parity
temp = byte ;
temp >>= 4 ;
temp = byte ^ temp ;
temp1 = temp ;
temp1 >>= 2 ;
temp = temp ^ temp1 ;
temp1 = temp ;
temp1 <<= 1 ;
temp ^= temp1 ;
temp &= 0x02 ;
#ifdef INVERT_SERIAL
byteLo ^= temp ;
#else
byteLo |= temp ;
#endif
}
else
{
byteLo |= 0xFE ; // Stop bit
}
byte <<= 1 ;
#ifdef INVERT_SERIAL
byte |= 1 ; // Start bit
#endif
uint8_t next = (SerialControl.head + 2) & 0x3f ;
if ( next != SerialControl.tail )
{
SerialControl.data[SerialControl.head] = byte ;
SerialControl.data[SerialControl.head+1] = byteLo ;
SerialControl.head = next ;
}
if(!IS_TX_PAUSE_on)
tx_resume();
}
void resumeBashSerial()
{
cli() ;
if ( SerialControl.busy == 0 )
{
sei() ;
// Start the transmission here
#ifdef INVERT_SERIAL
GPIOR2 = 0 ;
#else
GPIOR2 = 0x01 ;
#endif
if ( SerialControl.speed == SPEED_100K )
{
GPIOR1 = 1 ;
OCR0B = TCNT0 + 40 ;
OCR0A = OCR0B + 210 ;
TIFR0 = (1<>= 1
GPIOR0 = byte ;
if ( --GPIOR1 == 0 )
{
TIMSK0 &= ~(1<>= 1
GPIOR2 = byte ;
if ( --GPIOR1 == 0 )
{
if ( IS_TX_PAUSE_on )
{
SerialControl.busy = 0 ;
TIMSK0 &= ~(1<head != ptr->tail )
{
GPIOR0 = ptr->data[ptr->tail] ;
GPIOR2 = ptr->data[ptr->tail+1] ;
ptr->tail = ( ptr->tail + 2 ) & 0x3F ;
GPIOR1 = 8 ;
OCR0A = OCR0B + 40 ;
OCR0B = OCR0A + 8 * 20 ;
TIMSK0 |= (1< 2 )
{
byte = GPIOR0 ;
}
else
{
byte = GPIOR2 ;
}
if ( byte & 0x01 )
SERIAL_TX_on;
else
SERIAL_TX_off;
byte /= 2 ; // Generates shorter code than byte >>= 1
if ( GPIOR1 > 2 )
{
GPIOR0 = byte ;
}
else
{
GPIOR2 = byte ;
}
if ( --GPIOR1 == 0 )
{
// prepare next byte
volatile struct t_serial_bash *ptr = &SerialControl ;
if ( ptr->head != ptr->tail )
{
GPIOR0 = ptr->data[ptr->tail] ;
GPIOR2 = ptr->data[ptr->tail+1] ;
ptr->tail = ( ptr->tail + 2 ) & 0x3F ;
GPIOR1 = 10 ;
}
else
{
SerialControl.busy = 0 ;
TIMSK0 &= ~(1<