[9XR-PRO] Telemetry fix

radio/src/targets/sky9x/board_sky9x.h

@@ -293,6 +293,6 @@ void telemetryTransmitBuffer(uint8_t * buffer, uint32_t size);
 
 // Second UART driver
 void telemetrySecondPortInit(unsigned int protocol);
-uint16_t telemetrySecondPortReceive();
+bool telemetrySecondPortReceive(uint8_t & data);
 
 #endif

radio/src/targets/sky9x/second_serial_driver.cpp

@@ -44,10 +44,9 @@
 /** Pins description corresponding to Rxd,Txd, (UART pins) */
 #define SECOND_SERIAL_PINS        {PINS_UART}
 
+Fifo<512> serial2RxFifo;
+
 #if !defined(SIMU)
-
-extern Fifo<512> debugRxFifo;
-
 /*
  * 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 */
   pUart->UART_CR = UART_CR_RXEN | UART_CR_TXEN;
 
-#if defined(DEBUG)
   pUart->UART_IER = UART_IER_RXRDY;
   NVIC_EnableIRQ(UART0_IRQn);
-#endif
 }
 
 void SECOND_UART_Stop()
@@ -113,9 +110,7 @@ void SECOND_UART_Stop()
 
 extern "C" void UART0_IRQHandler()
 {
-#if defined(DEBUG)
-  debugRxFifo.push(SECOND_SERIAL_UART->UART_RHR);
-#endif
+  serial2RxFifo.push(SECOND_SERIAL_UART->UART_RHR);
 }
 #else
 #define SECOND_UART_Configure(...)
@@ -125,17 +120,11 @@ extern "C" void UART0_IRQHandler()
 void telemetrySecondPortInit(unsigned int /*protocol*/)
 {
   SECOND_UART_Configure(FRSKY_D_BAUDRATE, Master_frequency);
-  startPdcUsartReceive();
+  // startPdcUsartReceive();
 }
 
-uint16_t telemetrySecondPortReceive()
+bool telemetrySecondPortReceive(uint8_t & data)
 {
-  Uart *pUart = SECOND_SERIAL_UART;
-
-  if (pUart->UART_SR & UART_SR_RXRDY) {
-    return pUart->UART_RHR ;
-  }
-
-  return 0xFFFF ;
+  return serial2RxFifo.pop(data);
 }
 #endif

radio/src/targets/sky9x/telemetry_driver.cpp

@@ -36,7 +36,7 @@
 
 #include "../../opentx.h"
 
-#define RX_UART_BUFFER_SIZE     32
+#define RX_UART_BUFFER_SIZE     128
 
 struct t_rxUartBuffer
 {
@@ -44,8 +44,7 @@ struct t_rxUartBuffer
   uint8_t *outPtr ;
 };
 
-struct t_rxUartBuffer TelemetryInBuffer[2] ;
-uint32_t TelemetryActiveBuffer;
+struct t_rxUartBuffer TelemetryInBuffer ;
 uint16_t DsmRxTimeout;
 
 // USART0 configuration
@@ -106,18 +105,14 @@ extern "C" void USART0_IRQHandler()
 void startPdcUsartReceive()
 {
   register Usart *pUsart = SECOND_USART;
-
-  TelemetryInBuffer[0].outPtr = TelemetryInBuffer[0].fifo ;
-  TelemetryInBuffer[1].outPtr = TelemetryInBuffer[1].fifo ;
+  TelemetryInBuffer.outPtr = TelemetryInBuffer.fifo ;
 #ifndef SIMU
-  // TODO because of the 64bits cast ...
-  pUsart->US_RPR = (uint32_t)TelemetryInBuffer[0].fifo ;
-  pUsart->US_RNPR = (uint32_t)TelemetryInBuffer[1].fifo ;
+  pUsart->US_RPR = (uint32_t)TelemetryInBuffer.fifo ;
+  pUsart->US_RNPR = (uint32_t)TelemetryInBuffer.fifo ;
 #endif
   pUsart->US_RCR = RX_UART_BUFFER_SIZE ;
   pUsart->US_RNCR = RX_UART_BUFFER_SIZE ;
   pUsart->US_PTCR = US_PTCR_RXTEN ;
-  TelemetryActiveBuffer = 0;
 }
 
 void rxPdcUsart( void (*pChProcess)(uint8_t x) )
@@ -126,34 +121,29 @@ void rxPdcUsart( void (*pChProcess)(uint8_t x) )
   register Usart *pUsart = SECOND_USART;
   uint8_t *ptr ;
   uint8_t *endPtr ;
-  uint32_t j ;
 
   // Find out where the DMA has got to
-  __disable_irq() ;
-  pUsart->US_PTCR = US_PTCR_RXTDIS ;              // Freeze DMA
-  ptr = (uint8_t *)pUsart->US_RPR ;
-  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 = (uint8_t *)pUsart->US_RPR ;
+  // Check for DMA passed end of buffer
+  if ( endPtr > &TelemetryInBuffer.fifo[RX_UART_BUFFER_SIZE-1] )
   {
-    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++) ;
+    if ( ptr > &TelemetryInBuffer.fifo[RX_UART_BUFFER_SIZE-1] )       // last byte
+    {
+      ptr = TelemetryInBuffer.fifo ;
+    }
   }
-  TelemetryInBuffer[TelemetryActiveBuffer].outPtr = ptr ;
-  if ( j == 0 )           // First buf is full
+  TelemetryInBuffer.outPtr = ptr ;
+
+  if ( pUsart->US_RNCR == 0 )
   {
-    TelemetryInBuffer[TelemetryActiveBuffer].outPtr = TelemetryInBuffer[TelemetryActiveBuffer].fifo ;
-    pUsart->US_RNPR = (uint32_t)TelemetryInBuffer[TelemetryActiveBuffer].fifo ;
+    pUsart->US_RNPR = (uint32_t)TelemetryInBuffer.fifo ;
     pUsart->US_RNCR = RX_UART_BUFFER_SIZE ;
-    TelemetryActiveBuffer ^= 1 ;            // Other buffer is active
-    rxPdcUsart( pChProcess ) ;                      // Get any chars from second buffer
   }
 #endif
 }

radio/src/telemetry/frsky.cpp

@@ -277,7 +277,7 @@ enum AlarmsCheckSteps {
 void telemetryWakeup()
 {
 #if defined(CPUARM)
-  uint8_t requiredTelemetryProtocol = MODEL_TELEMETRY_PROTOCOL;
+  uint8_t requiredTelemetryProtocol = MODEL_TELEMETRY_PROTOCOL();
   if (telemetryProtocol != requiredTelemetryProtocol) {
     telemetryProtocol = requiredTelemetryProtocol;
     telemetryInit();
@@ -307,8 +307,8 @@ void telemetryWakeup()
   }
 #elif defined(PCBSKY9X)
   if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) {
-    uint16_t data ;
-    while ((data=telemetrySecondPortReceive()) != 0xFFFF) {
+    uint8_t data;
+    while (telemetrySecondPortReceive(data)) {
       processSerialData(data);
     }
   }

radio/src/telemetry/frsky.h

@@ -477,9 +477,9 @@ void frskyUpdateCells(void);
 #endif
 
 #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)
-  #define MODEL_TELEMETRY_PROTOCOL g_model.telemetryProtocol
+  #define MODEL_TELEMETRY_PROTOCOL() g_model.telemetryProtocol
 #endif
 
 #endif