From 58382d8aca3c0baa6465fe154cd665b3d6236abf Mon Sep 17 00:00:00 2001 From: Bertrand Songis Date: Mon, 9 Jan 2017 18:17:19 +0100 Subject: [PATCH] [Horus] Fixes #4205 (#4210) [Horus] Fixes #4205 --- radio/src/io/frsky_sport.cpp | 2 +- radio/src/targets/horus/board.cpp | 9 -- radio/src/targets/horus/board.h | 5 +- radio/src/targets/horus/telemetry_driver.cpp | 115 +++++++++++------- radio/src/targets/sky9x/board.h | 2 +- radio/src/targets/sky9x/telemetry_driver.cpp | 2 +- radio/src/targets/taranis/board.h | 4 +- .../src/targets/taranis/telemetry_driver.cpp | 9 +- radio/src/telemetry/telemetry.cpp | 26 ++-- radio/src/telemetry/telemetry.h | 7 +- 10 files changed, 97 insertions(+), 84 deletions(-) diff --git a/radio/src/io/frsky_sport.cpp b/radio/src/io/frsky_sport.cpp index 5402da02b..83e9fcdd7 100644 --- a/radio/src/io/frsky_sport.cpp +++ b/radio/src/io/frsky_sport.cpp @@ -176,7 +176,7 @@ const char * sportUpdatePowerOn(ModuleIndex module) sportWaitState(SPORT_IDLE, 500); // Clear the fifo - telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_8N1); + telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA); #if defined(PCBTARANIS) || defined(PCBHORUS) if (module == INTERNAL_MODULE) diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index 10b7b2adc..86a781f25 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -87,15 +87,6 @@ void interrupt1ms() DEBUG_TIMER_START(debugTimerRotEnc); checkRotaryEncoder(); DEBUG_TIMER_STOP(debugTimerRotEnc); - -#if defined(LUA) - if (telemetryProtocol == PROTOCOL_FRSKY_SPORT && outputTelemetryBufferTrigger && outputTelemetryBufferSize > 0) { - if (telemetryFifo.last(-2) == 0x7E && telemetryFifo.last(-1) == outputTelemetryBufferTrigger) { - outputTelemetryBufferTrigger = 0; - sportSendBuffer(outputTelemetryBuffer, outputTelemetryBufferSize); - } - } -#endif } extern "C" void INTERRUPT_1MS_IRQHandler() diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h index 83e979868..55b0d3774 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -400,10 +400,11 @@ int32_t getVolume(void); #define VOLUME_LEVEL_DEF 12 // Telemetry driver -void telemetryPortInit(uint32_t baudrate, int mode); +#define TELEMETRY_FIFO_SIZE 512 +void telemetryPortInit(uint32_t baudrate, uint8_t mode); void telemetryPortSetDirectionOutput(void); void sportSendBuffer(uint8_t * buffer, uint32_t count); -int telemetryGetByte(uint8_t * byte); +uint8_t telemetryGetByte(uint8_t * byte); // Haptic driver void hapticInit(void); diff --git a/radio/src/targets/horus/telemetry_driver.cpp b/radio/src/targets/horus/telemetry_driver.cpp index f8176ab73..e64ca5a3e 100644 --- a/radio/src/targets/horus/telemetry_driver.cpp +++ b/radio/src/targets/horus/telemetry_driver.cpp @@ -20,9 +20,12 @@ #include "opentx.h" -DMAFifo<512> telemetryFifo __DMA (TELEMETRY_DMA_Stream_RX); +DMAFifo telemetryDMAFifo __DMA (TELEMETRY_DMA_Stream_RX); +Fifo telemetryNoDMAFifo; +uint8_t telemetryFifoMode; +uint32_t telemetryErrors = 0; -void telemetryPortInit(uint32_t baudrate, int mode) +void telemetryPortInit(uint32_t baudrate, uint8_t mode) { if (baudrate == 0) { USART_DeInit(TELEMETRY_USART); @@ -56,7 +59,7 @@ void telemetryPortInit(uint32_t baudrate, int mode) GPIO_ResetBits(TELEMETRY_DIR_GPIO, TELEMETRY_DIR_GPIO_PIN); USART_InitStructure.USART_BaudRate = baudrate; - if (mode == TELEMETRY_SERIAL_8E2) { + if (mode & TELEMETRY_SERIAL_8E2) { USART_InitStructure.USART_WordLength = USART_WordLength_9b; USART_InitStructure.USART_StopBits = USART_StopBits_2; USART_InitStructure.USART_Parity = USART_Parity_Even; @@ -69,34 +72,44 @@ void telemetryPortInit(uint32_t baudrate, int mode) USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; USART_Init(TELEMETRY_USART, &USART_InitStructure); - - DMA_InitTypeDef DMA_InitStructure; - telemetryFifo.clear(); - USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, DISABLE); - USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE); - NVIC_SetPriority(TELEMETRY_USART_IRQn, 6); - NVIC_EnableIRQ(TELEMETRY_USART_IRQn); + telemetryFifoMode = mode; - DMA_InitStructure.DMA_Channel = TELEMETRY_DMA_Channel_RX; - DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&TELEMETRY_USART->DR); - DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(telemetryFifo.buffer()); - DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; - DMA_InitStructure.DMA_BufferSize = telemetryFifo.size(); - DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; - DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; - DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; - DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; - DMA_InitStructure.DMA_Priority = DMA_Priority_Low; - DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; - DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; - DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; - DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(TELEMETRY_DMA_Stream_RX, &DMA_InitStructure); - USART_DMACmd(TELEMETRY_USART, USART_DMAReq_Rx, ENABLE); - USART_Cmd(TELEMETRY_USART, ENABLE); - DMA_Cmd(TELEMETRY_DMA_Stream_RX, ENABLE); + if (mode & TELEMETRY_SERIAL_WITHOUT_DMA) { + USART_Cmd(TELEMETRY_USART, ENABLE); + USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, ENABLE); + NVIC_SetPriority(TELEMETRY_USART_IRQn, 6); + NVIC_EnableIRQ(TELEMETRY_USART_IRQn); + } + else { + DMA_InitTypeDef DMA_InitStructure; + telemetryDMAFifo.clear(); + + USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, DISABLE); + USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE); + NVIC_SetPriority(TELEMETRY_USART_IRQn, 6); + NVIC_EnableIRQ(TELEMETRY_USART_IRQn); + + DMA_InitStructure.DMA_Channel = TELEMETRY_DMA_Channel_RX; + DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&TELEMETRY_USART->DR); + DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(telemetryDMAFifo.buffer()); + DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; + DMA_InitStructure.DMA_BufferSize = telemetryDMAFifo.size(); + DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; + DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; + DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; + DMA_InitStructure.DMA_Priority = DMA_Priority_Low; + DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable; + DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full; + DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; + DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + DMA_Init(TELEMETRY_DMA_Stream_RX, &DMA_InitStructure); + USART_DMACmd(TELEMETRY_USART, USART_DMAReq_Rx, ENABLE); + USART_Cmd(TELEMETRY_USART, ENABLE); + DMA_Cmd(TELEMETRY_DMA_Stream_RX, ENABLE); + } } void telemetryPortSetDirectionOutput() @@ -160,7 +173,7 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) { DEBUG_INTERRUPT(INT_TELEM_USART); uint32_t status = TELEMETRY_USART->SR; - + if ((status & USART_SR_TC) && (TELEMETRY_USART->CR1 & USART_CR1_TCIE)) { TELEMETRY_USART->CR1 &= ~USART_CR1_TCIE; telemetryPortSetDirectionInput(); @@ -169,25 +182,33 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) status = TELEMETRY_USART->SR; } } + + while (status & (USART_FLAG_RXNE | USART_FLAG_ERRORS)) { + uint8_t data = TELEMETRY_USART->DR; + if (status & USART_FLAG_ERRORS) { + telemetryErrors++; + } + else { + telemetryNoDMAFifo.push(data); +#if defined(LUA) + if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) { + static uint8_t prevdata; + if (prevdata == 0x7E && outputTelemetryBufferSize > 0 && data == outputTelemetryBufferTrigger) { + sportSendBuffer(outputTelemetryBuffer, outputTelemetryBufferSize); + } + prevdata = data; + } +#endif + } + status = TELEMETRY_USART->SR; + } } // TODO we should have telemetry in an higher layer, functions above should move to a sport_driver.cpp -int telemetryGetByte(uint8_t * byte) +uint8_t telemetryGetByte(uint8_t * byte) { - // TODO I am not sure it's possible to have this cable on Horus ... -#if 0 - if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) { - if (serial2Mode == UART_MODE_TELEMETRY) - return serial2RxFifo.pop(*byte); - else - return 0; - } - else { - return telemetryFifo.pop(*byte); - } -#else - return telemetryFifo.pop(*byte); -#endif - - + if (telemetryFifoMode & TELEMETRY_SERIAL_WITHOUT_DMA) + return telemetryNoDMAFifo.pop(*byte); + else + return telemetryDMAFifo.pop(*byte); } diff --git a/radio/src/targets/sky9x/board.h b/radio/src/targets/sky9x/board.h index 195b8a9f7..a849b24e5 100644 --- a/radio/src/targets/sky9x/board.h +++ b/radio/src/targets/sky9x/board.h @@ -376,7 +376,7 @@ void eepromStartWrite(uint8_t * buffer, size_t address, size_t size); void debugPutc(const char c); // Telemetry driver -void telemetryPortInit(uint32_t baudrate, int mode); +void telemetryPortInit(uint32_t baudrate, uint8_t mode); uint32_t telemetryTransmitPending(); void telemetryTransmitBuffer(uint8_t * buffer, uint32_t size); void rxPdcUsart( void (*pChProcess)(uint8_t x) ); diff --git a/radio/src/targets/sky9x/telemetry_driver.cpp b/radio/src/targets/sky9x/telemetry_driver.cpp index d2eedb469..4446ee37a 100644 --- a/radio/src/targets/sky9x/telemetry_driver.cpp +++ b/radio/src/targets/sky9x/telemetry_driver.cpp @@ -166,7 +166,7 @@ uint32_t telemetryTransmitPending() return x ; } -void telemetryPortInit(uint32_t baudrate, int mode) +void telemetryPortInit(uint32_t baudrate, uint8_t mode) { #if !defined(SIMU) UART2_Configure(baudrate, Master_frequency, mode); diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h index 69607e79e..be46084b3 100644 --- a/radio/src/targets/taranis/board.h +++ b/radio/src/targets/taranis/board.h @@ -457,10 +457,10 @@ uint8_t eepromIsTransferComplete(); void debugPutc(const char c); // Telemetry driver -void telemetryPortInit(uint32_t baudrate, int mode); +void telemetryPortInit(uint32_t baudrate, uint8_t mode); void telemetryPortSetDirectionOutput(void); void sportSendBuffer(uint8_t * buffer, uint32_t count); -int telemetryGetByte(uint8_t * byte); +uint8_t telemetryGetByte(uint8_t * byte); extern uint32_t telemetryErrors; // Audio driver diff --git a/radio/src/targets/taranis/telemetry_driver.cpp b/radio/src/targets/taranis/telemetry_driver.cpp index 4f3035094..b40465a98 100644 --- a/radio/src/targets/taranis/telemetry_driver.cpp +++ b/radio/src/targets/taranis/telemetry_driver.cpp @@ -23,7 +23,7 @@ Fifo telemetryFifo; uint32_t telemetryErrors = 0; -void telemetryPortInit(uint32_t baudrate, int mode) +void telemetryPortInit(uint32_t baudrate, uint8_t mode) { if (baudrate == 0) { USART_DeInit(TELEMETRY_USART); @@ -57,7 +57,7 @@ void telemetryPortInit(uint32_t baudrate, int mode) GPIO_ResetBits(TELEMETRY_DIR_GPIO, TELEMETRY_DIR_GPIO_PIN); USART_InitStructure.USART_BaudRate = baudrate; - if (mode == TELEMETRY_SERIAL_8E2) { + if (mode & TELEMETRY_SERIAL_8E2) { USART_InitStructure.USART_WordLength = USART_WordLength_9b; USART_InitStructure.USART_StopBits = USART_StopBits_2; USART_InitStructure.USART_Parity = USART_Parity_Even; @@ -70,6 +70,7 @@ void telemetryPortInit(uint32_t baudrate, int mode) USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx; USART_Init(TELEMETRY_USART, &USART_InitStructure); + USART_Cmd(TELEMETRY_USART, ENABLE); USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, ENABLE); NVIC_SetPriority(TELEMETRY_USART_IRQn, 6); @@ -169,14 +170,14 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) } // TODO we should have telemetry in an higher layer, functions above should move to a sport_driver.cpp -int telemetryGetByte(uint8_t * byte) +uint8_t telemetryGetByte(uint8_t * byte) { #if defined(SERIAL2) if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) { if (serial2Mode == UART_MODE_TELEMETRY) return serial2RxFifo.pop(*byte); else - return 0; + return false; } else { return telemetryFifo.pop(*byte); diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index cd11dacbe..39c2b8fa0 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -104,12 +104,12 @@ void telemetryWakeup() #if defined(STM32) uint8_t data; - if (!telemetryFifo.isEmpty()) { + if (telemetryGetByte(&data)) { LOG_TELEMETRY_WRITE_START(); - } - while (telemetryFifo.pop(data)) { - processTelemetryData(data); - LOG_TELEMETRY_WRITE_BYTE(data); + do { + processTelemetryData(data); + LOG_TELEMETRY_WRITE_BYTE(data); + } while (telemetryGetByte(&data)); } #elif defined(PCBSKY9X) if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) { @@ -416,18 +416,18 @@ void telemetryInit(uint8_t protocol) { if (protocol == PROTOCOL_MULTIMODULE || protocol == PROTOCOL_FLYSKY_IBUS) { // The DIY Multi module always speaks 100000 baud regardless of the telemetry protocol in use telemetryPortInit(MULTIMODULE_BAUDRATE, TELEMETRY_SERIAL_8E2); - } else if (protocol == PROTOCOL_SPEKTRUM) - { - // Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same since there is no real standard - telemetryPortInit(125000, TELEMETRY_SERIAL_8N1); + } + else if (protocol == PROTOCOL_SPEKTRUM) { + // Spektrum's own small race RX (SPM4648) uses 125000 8N1, use the same since there is no real standard + telemetryPortInit(125000, TELEMETRY_SERIAL_DEFAULT); } else #endif if (protocol == PROTOCOL_FRSKY_D) { - telemetryPortInit(FRSKY_D_BAUDRATE, TELEMETRY_SERIAL_8N1); + telemetryPortInit(FRSKY_D_BAUDRATE, TELEMETRY_SERIAL_DEFAULT); } #if defined(CROSSFIRE) else if (protocol == PROTOCOL_PULSES_CROSSFIRE) { - telemetryPortInit(CROSSFIRE_BAUDRATE, TELEMETRY_SERIAL_8N1); + telemetryPortInit(CROSSFIRE_BAUDRATE, TELEMETRY_SERIAL_DEFAULT); #if defined(LUA) outputTelemetryBufferSize = 0; outputTelemetryBufferTrigger = 0; @@ -437,12 +437,12 @@ void telemetryInit(uint8_t protocol) { #endif #if defined(SERIAL2) else if (protocol == PROTOCOL_FRSKY_D_SECONDARY) { - telemetryPortInit(0, TELEMETRY_SERIAL_8N1); + telemetryPortInit(0, TELEMETRY_SERIAL_DEFAULT); serial2TelemetryInit(PROTOCOL_FRSKY_D_SECONDARY); } #endif else { - telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_8N1); + telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA); #if defined(LUA) outputTelemetryBufferSize = 0; outputTelemetryBufferTrigger = 0x7E; diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index 595b02af8..78ce690b8 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -66,10 +66,9 @@ extern uint8_t telemetryState; #define TELEMETRY_TIMEOUT10ms 100 // 1 second -enum TelemetrySerialMode { - TELEMETRY_SERIAL_8N1, - TELEMETRY_SERIAL_8E2 -}; +#define TELEMETRY_SERIAL_DEFAULT 0 +#define TELEMETRY_SERIAL_8E2 1 +#define TELEMETRY_SERIAL_WITHOUT_DMA 2 #if defined(CROSSFIRE) || defined(MULTIMODULE) #define TELEMETRY_RX_PACKET_SIZE 128