[9XR-PRO] Telemetry fix
This commit is contained in:
@@ -293,6 +293,6 @@ void telemetryTransmitBuffer(uint8_t * buffer, uint32_t size);
|
|||||||
|
|
||||||
// Second UART driver
|
// Second UART driver
|
||||||
void telemetrySecondPortInit(unsigned int protocol);
|
void telemetrySecondPortInit(unsigned int protocol);
|
||||||
uint16_t telemetrySecondPortReceive();
|
bool telemetrySecondPortReceive(uint8_t & data);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -44,10 +44,9 @@
|
|||||||
/** Pins description corresponding to Rxd,Txd, (UART pins) */
|
/** Pins description corresponding to Rxd,Txd, (UART pins) */
|
||||||
#define SECOND_SERIAL_PINS {PINS_UART}
|
#define SECOND_SERIAL_PINS {PINS_UART}
|
||||||
|
|
||||||
|
Fifo<512> serial2RxFifo;
|
||||||
|
|
||||||
#if !defined(SIMU)
|
#if !defined(SIMU)
|
||||||
|
|
||||||
extern Fifo<512> debugRxFifo;
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Outputs a character on the UART line.
|
* Outputs a character on the UART line.
|
||||||
*
|
*
|
||||||
@@ -99,10 +98,8 @@ void SECOND_UART_Configure(uint32_t baudrate, uint32_t masterClock)
|
|||||||
/* Enable receiver and transmitter */
|
/* Enable receiver and transmitter */
|
||||||
pUart->UART_CR = UART_CR_RXEN | UART_CR_TXEN;
|
pUart->UART_CR = UART_CR_RXEN | UART_CR_TXEN;
|
||||||
|
|
||||||
#if defined(DEBUG)
|
|
||||||
pUart->UART_IER = UART_IER_RXRDY;
|
pUart->UART_IER = UART_IER_RXRDY;
|
||||||
NVIC_EnableIRQ(UART0_IRQn);
|
NVIC_EnableIRQ(UART0_IRQn);
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void SECOND_UART_Stop()
|
void SECOND_UART_Stop()
|
||||||
@@ -113,9 +110,7 @@ void SECOND_UART_Stop()
|
|||||||
|
|
||||||
extern "C" void UART0_IRQHandler()
|
extern "C" void UART0_IRQHandler()
|
||||||
{
|
{
|
||||||
#if defined(DEBUG)
|
serial2RxFifo.push(SECOND_SERIAL_UART->UART_RHR);
|
||||||
debugRxFifo.push(SECOND_SERIAL_UART->UART_RHR);
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
#define SECOND_UART_Configure(...)
|
#define SECOND_UART_Configure(...)
|
||||||
@@ -125,17 +120,11 @@ extern "C" void UART0_IRQHandler()
|
|||||||
void telemetrySecondPortInit(unsigned int /*protocol*/)
|
void telemetrySecondPortInit(unsigned int /*protocol*/)
|
||||||
{
|
{
|
||||||
SECOND_UART_Configure(FRSKY_D_BAUDRATE, Master_frequency);
|
SECOND_UART_Configure(FRSKY_D_BAUDRATE, Master_frequency);
|
||||||
startPdcUsartReceive();
|
// startPdcUsartReceive();
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t telemetrySecondPortReceive()
|
bool telemetrySecondPortReceive(uint8_t & data)
|
||||||
{
|
{
|
||||||
Uart *pUart = SECOND_SERIAL_UART;
|
return serial2RxFifo.pop(data);
|
||||||
|
|
||||||
if (pUart->UART_SR & UART_SR_RXRDY) {
|
|
||||||
return pUart->UART_RHR ;
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0xFFFF ;
|
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -36,7 +36,7 @@
|
|||||||
|
|
||||||
#include "../../opentx.h"
|
#include "../../opentx.h"
|
||||||
|
|
||||||
#define RX_UART_BUFFER_SIZE 32
|
#define RX_UART_BUFFER_SIZE 128
|
||||||
|
|
||||||
struct t_rxUartBuffer
|
struct t_rxUartBuffer
|
||||||
{
|
{
|
||||||
@@ -44,8 +44,7 @@ struct t_rxUartBuffer
|
|||||||
uint8_t *outPtr ;
|
uint8_t *outPtr ;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct t_rxUartBuffer TelemetryInBuffer[2] ;
|
struct t_rxUartBuffer TelemetryInBuffer ;
|
||||||
uint32_t TelemetryActiveBuffer;
|
|
||||||
uint16_t DsmRxTimeout;
|
uint16_t DsmRxTimeout;
|
||||||
|
|
||||||
// USART0 configuration
|
// USART0 configuration
|
||||||
@@ -106,18 +105,14 @@ extern "C" void USART0_IRQHandler()
|
|||||||
void startPdcUsartReceive()
|
void startPdcUsartReceive()
|
||||||
{
|
{
|
||||||
register Usart *pUsart = SECOND_USART;
|
register Usart *pUsart = SECOND_USART;
|
||||||
|
TelemetryInBuffer.outPtr = TelemetryInBuffer.fifo ;
|
||||||
TelemetryInBuffer[0].outPtr = TelemetryInBuffer[0].fifo ;
|
|
||||||
TelemetryInBuffer[1].outPtr = TelemetryInBuffer[1].fifo ;
|
|
||||||
#ifndef SIMU
|
#ifndef SIMU
|
||||||
// TODO because of the 64bits cast ...
|
pUsart->US_RPR = (uint32_t)TelemetryInBuffer.fifo ;
|
||||||
pUsart->US_RPR = (uint32_t)TelemetryInBuffer[0].fifo ;
|
pUsart->US_RNPR = (uint32_t)TelemetryInBuffer.fifo ;
|
||||||
pUsart->US_RNPR = (uint32_t)TelemetryInBuffer[1].fifo ;
|
|
||||||
#endif
|
#endif
|
||||||
pUsart->US_RCR = RX_UART_BUFFER_SIZE ;
|
pUsart->US_RCR = RX_UART_BUFFER_SIZE ;
|
||||||
pUsart->US_RNCR = RX_UART_BUFFER_SIZE ;
|
pUsart->US_RNCR = RX_UART_BUFFER_SIZE ;
|
||||||
pUsart->US_PTCR = US_PTCR_RXTEN ;
|
pUsart->US_PTCR = US_PTCR_RXTEN ;
|
||||||
TelemetryActiveBuffer = 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void rxPdcUsart( void (*pChProcess)(uint8_t x) )
|
void rxPdcUsart( void (*pChProcess)(uint8_t x) )
|
||||||
@@ -126,34 +121,29 @@ void rxPdcUsart( void (*pChProcess)(uint8_t x) )
|
|||||||
register Usart *pUsart = SECOND_USART;
|
register Usart *pUsart = SECOND_USART;
|
||||||
uint8_t *ptr ;
|
uint8_t *ptr ;
|
||||||
uint8_t *endPtr ;
|
uint8_t *endPtr ;
|
||||||
uint32_t j ;
|
|
||||||
|
|
||||||
// Find out where the DMA has got to
|
// Find out where the DMA has got to
|
||||||
__disable_irq() ;
|
endPtr = (uint8_t *)pUsart->US_RPR ;
|
||||||
pUsart->US_PTCR = US_PTCR_RXTDIS ; // Freeze DMA
|
// Check for DMA passed end of buffer
|
||||||
ptr = (uint8_t *)pUsart->US_RPR ;
|
if ( endPtr > &TelemetryInBuffer.fifo[RX_UART_BUFFER_SIZE-1] )
|
||||||
j = pUsart->US_RNCR ;
|
|
||||||
pUsart->US_PTCR = US_PTCR_RXTEN ; // DMA active again
|
|
||||||
__enable_irq() ;
|
|
||||||
|
|
||||||
endPtr = ptr - 1 ;
|
|
||||||
ptr = TelemetryInBuffer[TelemetryActiveBuffer].outPtr ;
|
|
||||||
if ( j == 0 ) // First buf is full
|
|
||||||
{
|
{
|
||||||
endPtr = &TelemetryInBuffer[TelemetryActiveBuffer].fifo[RX_UART_BUFFER_SIZE-1] ; // last byte
|
endPtr = TelemetryInBuffer.fifo ;
|
||||||
}
|
}
|
||||||
while ( ptr <= endPtr )
|
ptr = TelemetryInBuffer.outPtr ;
|
||||||
|
while ( ptr != endPtr )
|
||||||
{
|
{
|
||||||
(*pChProcess)(*ptr++) ;
|
(*pChProcess)(*ptr++) ;
|
||||||
|
if ( ptr > &TelemetryInBuffer.fifo[RX_UART_BUFFER_SIZE-1] ) // last byte
|
||||||
|
{
|
||||||
|
ptr = TelemetryInBuffer.fifo ;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
TelemetryInBuffer[TelemetryActiveBuffer].outPtr = ptr ;
|
TelemetryInBuffer.outPtr = ptr ;
|
||||||
if ( j == 0 ) // First buf is full
|
|
||||||
|
if ( pUsart->US_RNCR == 0 )
|
||||||
{
|
{
|
||||||
TelemetryInBuffer[TelemetryActiveBuffer].outPtr = TelemetryInBuffer[TelemetryActiveBuffer].fifo ;
|
pUsart->US_RNPR = (uint32_t)TelemetryInBuffer.fifo ;
|
||||||
pUsart->US_RNPR = (uint32_t)TelemetryInBuffer[TelemetryActiveBuffer].fifo ;
|
|
||||||
pUsart->US_RNCR = RX_UART_BUFFER_SIZE ;
|
pUsart->US_RNCR = RX_UART_BUFFER_SIZE ;
|
||||||
TelemetryActiveBuffer ^= 1 ; // Other buffer is active
|
|
||||||
rxPdcUsart( pChProcess ) ; // Get any chars from second buffer
|
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -277,7 +277,7 @@ enum AlarmsCheckSteps {
|
|||||||
void telemetryWakeup()
|
void telemetryWakeup()
|
||||||
{
|
{
|
||||||
#if defined(CPUARM)
|
#if defined(CPUARM)
|
||||||
uint8_t requiredTelemetryProtocol = MODEL_TELEMETRY_PROTOCOL;
|
uint8_t requiredTelemetryProtocol = MODEL_TELEMETRY_PROTOCOL();
|
||||||
if (telemetryProtocol != requiredTelemetryProtocol) {
|
if (telemetryProtocol != requiredTelemetryProtocol) {
|
||||||
telemetryProtocol = requiredTelemetryProtocol;
|
telemetryProtocol = requiredTelemetryProtocol;
|
||||||
telemetryInit();
|
telemetryInit();
|
||||||
@@ -307,8 +307,8 @@ void telemetryWakeup()
|
|||||||
}
|
}
|
||||||
#elif defined(PCBSKY9X)
|
#elif defined(PCBSKY9X)
|
||||||
if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) {
|
if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) {
|
||||||
uint16_t data ;
|
uint8_t data;
|
||||||
while ((data=telemetrySecondPortReceive()) != 0xFFFF) {
|
while (telemetrySecondPortReceive(data)) {
|
||||||
processSerialData(data);
|
processSerialData(data);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -477,9 +477,9 @@ void frskyUpdateCells(void);
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(PCBTARANIS)
|
#if defined(PCBTARANIS)
|
||||||
#define MODEL_TELEMETRY_PROTOCOL ((g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_OFF && g_model.externalModule == MODULE_TYPE_PPM) ? g_model.telemetryProtocol : PROTOCOL_FRSKY_SPORT)
|
#define MODEL_TELEMETRY_PROTOCOL() ((g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_OFF && g_model.externalModule == MODULE_TYPE_PPM) ? g_model.telemetryProtocol : PROTOCOL_FRSKY_SPORT)
|
||||||
#elif defined(CPUARM)
|
#elif defined(CPUARM)
|
||||||
#define MODEL_TELEMETRY_PROTOCOL g_model.telemetryProtocol
|
#define MODEL_TELEMETRY_PROTOCOL() g_model.telemetryProtocol
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
Reference in New Issue
Block a user