diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino index 08fc29b..aa124f5 100644 --- a/Multiprotocol/FrSkyX_cc2500.ino +++ b/Multiprotocol/FrSkyX_cc2500.ino @@ -1,4 +1,3 @@ - /* ************************** * By Midelic on RCGroups * ************************** @@ -164,6 +163,30 @@ static void __attribute__((unused)) frskyX_data_frame() uint8_t limit = (sub_protocol & 2 ) ? 31 : 28 ; for (uint8_t i=22;i>8;//high byte diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index 78d3636..cbcd780 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -19,7 +19,7 @@ #define VERSION_MAJOR 1 #define VERSION_MINOR 1 #define VERSION_REVISION 6 -#define VERSION_PATCH_LEVEL 37 +#define VERSION_PATCH_LEVEL 38 //****************** // Protocols //****************** diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index b768b15..22486e1 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -194,6 +194,13 @@ uint8_t pkt[MAX_PKT];//telemetry receiving packets uint8_t telemetry_link=0; uint8_t telemetry_counter=0; uint8_t telemetry_lost; + #ifdef SPORT_POLLING + #define MAX_SPORT_BUFFER 64 + uint8_t SportData[MAX_SPORT_BUFFER]; + bool ok_to_send = false; + uint8_t sport_idx = 0; + uint8_t sport_index = 0; + #endif #endif // TELEMETRY // Callback @@ -670,7 +677,9 @@ inline void tx_resume() { #ifdef TELEMETRY // Resume telemetry by enabling transmitter interrupt + #ifndef SPORT_POLLING if(!IS_TX_PAUSE_on) + #endif { #ifdef ORANGE_TX cli() ; @@ -1195,8 +1204,9 @@ void modules_reset() USART2_BASE->CR1 |= USART_CR1_PCE_BIT; } usart3_begin(100000,SERIAL_8E2); - - USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable + #ifndef SPORT_POLLING + USART3_BASE->CR1 &= ~ USART_CR1_RE; //disable + #endif USART2_BASE->CR1 &= ~ USART_CR1_TE; //disable transmit #else //ATMEGA328p diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index 04f0af0..3889450 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -48,6 +48,18 @@ uint8_t RetrySequence ; // Store for FrskyX telemetry struct t_fx_rx_frame FrskyxRxFrames[4] ; uint8_t NextFxFrameToForward ; + #ifdef SPORT_POLLING + uint8_t sport_rx_index[28] ; + uint8_t ukindex ; + uint8_t kindex ; + uint8_t TxData[2]; + uint8_t SportIndexPolling; + uint8_t RxData[16] ; + volatile uint8_t RxIndex=0 ; + uint8_t sport_bytes=0; + uint8_t skipped_id; + uint8_t rx_counter=0; + #endif #endif // SPORT_TELEMETRY #if defined HUB_TELEMETRY @@ -433,13 +445,15 @@ pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09 0x34 0x0A 0xC3 0x56 0xF3 */ +#if defined SPORT_POLLING || defined MULTI_TELEMETRY +const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45, + 0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB, + 0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71, + 0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7, + 0x98, 0x39, 0xBA, 0x1B } ; +#endif #ifdef MULTI_TELEMETRY - const uint8_t PROGMEM Indices[] = { 0x00, 0xA1, 0x22, 0x83, 0xE4, 0x45, - 0xC6, 0x67, 0x48, 0xE9, 0x6A, 0xCB, - 0xAC, 0x0D, 0x8E, 0x2F, 0xD0, 0x71, - 0xF2, 0x53, 0x34, 0x95, 0x16, 0xB7, - 0x98, 0x39, 0xBA, 0x1B } ; void sportSend(uint8_t *p) { multi_send_header(MULTI_TELEMETRY_SPORT, 9); @@ -489,8 +503,158 @@ pkt[6]|(counter++)|00 01 02 03 04 05 06 07 08 09 } } } + #endif +#if defined SPORT_POLLING +uint8_t nextID() +{ + uint8_t i ; + uint8_t poll_idx ; + if (phase) + { + poll_idx = 99 ; + for ( i = 0 ; i < 28 ; i++ ) + { + if ( sport_rx_index[kindex] ) + { + poll_idx = kindex ; + } + kindex++ ; + if ( kindex>= 28 ) + { + kindex = 0 ; + phase = 0 ; + break ; + } + if ( poll_idx != 99 ) + { + break ; + } + } + if ( poll_idx != 99 ) + { + return poll_idx ; + } + } + if ( phase == 0 ) + { + for ( i = 0 ; i < 28 ; i++ ) + { + if ( sport_rx_index[ukindex] == 0 ) + { + poll_idx = ukindex ; + phase = 1 ; + } + ukindex++; + if (ukindex >= 28 ) + { + ukindex = 0 ; + } + if ( poll_idx != 99 ) + { + return poll_idx ; + } + } + if ( poll_idx == 99 ) + { + phase = 1 ; + return 0 ; + } + } + return poll_idx ; +} + +void pollSport() +{ + uint8_t pindex = nextID() ; + TxData[0] = START_STOP; + TxData[1] = pgm_read_byte_near(&Indices[pindex]) ; + if(!telemetry_lost && ((TxData[1] &0x1F)== skipped_id ||TxData[1]==0x98)) + {//98 ID(RSSI/RxBat and SWR ) and ID's from sport telemetry + pindex = nextID() ; + TxData[1] = pgm_read_byte_near(&Indices[pindex]); + } + SportIndexPolling = pindex ; + RxIndex = 0; + Serial_write(TxData[0]); + Serial_write(TxData[1]); +} + +bool checkSportPacket() +{ + uint8_t *packet = RxData ; + uint16_t crc = 0 ; + if ( RxIndex < 8 ) + return 0 ; + for ( uint8_t i = 0 ; i<8 ; i += 1 ) + { + crc += packet[i]; + crc += crc >> 8; + crc &= 0x00ff; + } + return (crc == 0x00ff) ; +} + +uint8_t unstuff() +{ + uint8_t i ; + uint8_t j ; + j = 0 ; + for ( i = 0 ; i < RxIndex ; i += 1 ) + { + if ( RxData[i] == BYTESTUFF ) + { + i += 1 ; + RxData[j] = RxData[i] ^ STUFF_MASK ; ; + } + else + RxData[j] = RxData[i] ; + j += 1 ; + } + return j ; +} + +void processSportData(uint8_t *p) +{ + + RxIndex = unstuff() ; + uint8_t x=checkSportPacket() ; + if (x) + { + SportData[sport_idx]=0x7E; + sport_idx =(sport_idx+1) & (MAX_SPORT_BUFFER-1); + SportData[sport_idx]=TxData[1]&0x1F; + sport_idx =(sport_idx+1) & (MAX_SPORT_BUFFER-1); + + for(uint8_t i=0;i<(RxIndex-1);i++) + {//no crc + if(p[i]==START_STOP || p[i]==BYTESTUFF) + {//stuff back + SportData[sport_idx]=BYTESTUFF; + sport_idx =(sport_idx+1) & (MAX_SPORT_BUFFER-1); + SportData[sport_idx]=p[i]^STUFF_MASK; + } + else + SportData[sport_idx]=p[i]; + sport_idx =(sport_idx+1) & (MAX_SPORT_BUFFER-1); + } + sport_rx_index[SportIndexPolling] = 1 ; + ok_to_send=true; + RxIndex =0 ; + } +} + +inline void rx_pause() +{ + USART3_BASE->CR1 &= ~ USART_CR1_RXNEIE; //disable rx interrupt on USART3 +} +inline void rx_resume() +{ + USART3_BASE->CR1 |= USART_CR1_RXNEIE; //enable rx interrupt on USART3 +} +#endif//end SPORT_POLLING + void sportIdle() { #if !defined MULTI_TELEMETRY @@ -500,11 +664,18 @@ void sportIdle() void sportSendFrame() { + #if defined SPORT_POLLING + rx_pause(); + #endif uint8_t i; sport_counter = (sport_counter + 1) %36; if(telemetry_lost) { - sportIdle(); + #ifdef SPORT_POLLING + pollSport(); + #else + sportIdle(); + #endif return; } if(sport_counter<6) @@ -542,6 +713,9 @@ void sportSendFrame() for (i=0;i SPORT_TIME) { + #if defined SPORT_POLLING + processSportData(RxData); //process arrived data before polling + #endif sportSendFrame(); #ifdef STM32_BOARD last=now; @@ -831,21 +1012,39 @@ void TelemetryUpdate() #endif { // Transmit interrupt #ifdef STM32_BOARD + #ifdef SPORT_POLLING + if(USART3_BASE->SR & USART_SR_RXNE) + { + USART3_BASE->SR &= ~USART_SR_RXNE; + if (RxIndex < 16 ) + { + if(RxData[0]==TxData[0] && RxData[1]==TxData[1]) + RxIndex=0; + RxData[RxIndex++] = USART3_BASE->DR & 0xFF ; + } + } + #endif if(USART3_BASE->SR & USART_SR_TXE) { + USART3_BASE->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 + 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 SPORT_POLLING + rx_resume(); + #endif + } #ifdef STM32_BOARD } #endif diff --git a/Multiprotocol/Validate.h b/Multiprotocol/Validate.h index 02ba70b..b618d10 100644 --- a/Multiprotocol/Validate.h +++ b/Multiprotocol/Validate.h @@ -137,6 +137,10 @@ #endif #endif +#if not defined (SPORT_TELEMETRY) || not defined (STM32_BOARD) + #undef SPORT_POLLING +#endif + //Make sure TX is defined correctly #ifndef AILERON #error You must select a correct channel order. diff --git a/Multiprotocol/_Config.h b/Multiprotocol/_Config.h index b9695fd..782d62f 100644 --- a/Multiprotocol/_Config.h +++ b/Multiprotocol/_Config.h @@ -229,6 +229,12 @@ const int8_t AFHDS2AFailsafe[14]= { #define HUBSAN_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX #define CABELL_HUB_TELEMETRY // Use FrSkyD Hub format to send telemetry to TX +//SPORT_POLLING is an implementation of the same polling routine as XJT module for sport telemetry bidirectional communication. +//This is useful for passing sport control frames from Tx to rx(ex: changing Betaflight PID or VTX channels on the fly using LUA scripts with OpentX). +//Using this feature on turnigy 9XR_PRO requires uncomment INVERT_TELEMETRY as this TX output on telemetry pin only inverted signal. +//!This is a work in progress! +//#define SPORT_POLLING + /****************************/ /*** SERIAL MODE SETTINGS ***/ /****************************/