diff --git a/radio/src/fifo.h b/radio/src/fifo.h index e24b9ba80..a61d31839 100644 --- a/radio/src/fifo.h +++ b/radio/src/fifo.h @@ -21,6 +21,8 @@ #ifndef _FIFO_H_ #define _FIFO_H_ +#include + template class Fifo { @@ -40,13 +42,18 @@ class Fifo void push(T element) { - uint32_t next = (widx+1) & (N-1); + uint32_t next = nextIndex(widx); if (next != ridx) { fifo[widx] = element; widx = next; } } + void skip() + { + ridx = nextIndex(ridx); + } + bool pop(T & element) { if (isEmpty()) { @@ -54,7 +61,7 @@ class Fifo } else { element = fifo[ridx]; - ridx = (ridx+1) & (N-1); + ridx = nextIndex(ridx); return true; } } @@ -66,7 +73,7 @@ class Fifo bool isFull() { - uint32_t next = (widx+1) & (N-1); + uint32_t next = nextIndex(widx); return (next == ridx); } @@ -77,7 +84,7 @@ class Fifo uint32_t size() const { - return (N + widx - ridx) & (N-1); + return (N + widx - ridx) & (N - 1); } uint32_t hasSpace(uint32_t n) const @@ -100,6 +107,11 @@ class Fifo T fifo[N]; volatile uint32_t widx; volatile uint32_t ridx; + + inline uint32_t nextIndex(uint32_t idx) + { + return (idx + 1) & (N - 1); + } }; #endif // _FIFO_H_ diff --git a/radio/src/pulses/pulses.h b/radio/src/pulses/pulses.h index 21b8a035e..70fe6af50 100644 --- a/radio/src/pulses/pulses.h +++ b/radio/src/pulses/pulses.h @@ -80,7 +80,7 @@ PACK(struct ModuleSettings { uint8_t protocol:5; uint8_t paused:1; uint8_t mode:2; - uint16_t failsafeCounter; + uint16_t counter; }); extern ModuleSettings moduleSettings[NUM_MODULES]; @@ -195,12 +195,12 @@ inline bool pulsesStarted() { return moduleSettings[0].protocol != PROTOCOL_CHAN inline void pausePulses() { s_pulses_paused = true; } inline void resumePulses() { s_pulses_paused = false; } -#define SEND_FAILSAFE_NOW(idx) moduleSettings[idx].failsafeCounter = 1 +#define SEND_FAILSAFE_NOW(idx) moduleSettings[idx].counter = 1 inline void SEND_FAILSAFE_1S() { for (int i=0; i::addFlag1(uint8_t module) } else { bool failsafeNeeded = g_model.moduleData[module].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[module].failsafeMode != FAILSAFE_RECEIVER; - if (moduleSettings[module].failsafeCounter-- == 0) { - // failsafeCounter is also used for knowing if the frame is odd / even - moduleSettings[module].failsafeCounter = 1000; + if (moduleSettings[module].counter-- == 0) { + // counter is also used for knowing if the frame is odd / even + moduleSettings[module].counter = 1000; if (failsafeNeeded) { flag1 |= PXX_SEND_FAILSAFE; } } - if (failsafeNeeded && moduleSettings[module].failsafeCounter == 0 && g_model.moduleData[module].channelsCount > 0) { + if (failsafeNeeded && moduleSettings[module].counter == 0 && g_model.moduleData[module].channelsCount > 0) { flag1 |= PXX_SEND_FAILSAFE; } } @@ -120,7 +120,7 @@ void Pxx1Pulses::setupFrame(uint8_t module) } #else uint8_t sendUpperChannels = 0; - if (moduleSettings[module].failsafeCounter & 0x01) { + if (moduleSettings[module].counter & 0x01) { sendUpperChannels = g_model.moduleData[module].channelsCount; } add8ChannelsFrame(module, sendUpperChannels); diff --git a/radio/src/pulses/pxx2.cpp b/radio/src/pulses/pxx2.cpp index c468851e9..0239e5503 100644 --- a/radio/src/pulses/pxx2.cpp +++ b/radio/src/pulses/pxx2.cpp @@ -25,8 +25,8 @@ uint8_t Pxx2Pulses::addFlag0(uint8_t module) { uint8_t flag0 = g_model.header.modelId[module] & 0x3F; if (g_model.moduleData[module].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[module].failsafeMode != FAILSAFE_RECEIVER) { - if (moduleSettings[module].failsafeCounter-- == 0) { - moduleSettings[module].failsafeCounter = 1000; + if (moduleSettings[module].counter-- == 0) { + moduleSettings[module].counter = 1000; flag0 |= PXX2_FLAG0_FAILSAFE; } } @@ -57,8 +57,20 @@ void Pxx2Pulses::setupChannelsFrame(uint8_t module) void Pxx2Pulses::setupRegisterFrame(uint8_t module) { + unsigned counter = moduleSettings[module].counter; + addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_REGISTER); - Pxx2Transport::addByte(0); + + if (counter == REGISTER_COUNTER_ID_RECEIVED) { + Pxx2Transport::addByte(1); + for (uint8_t i=0; i<4; i++) { + Pxx2Transport::addByte(g_model.modelRegistrationID[i]); + } + } + else { + addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_REGISTER); + Pxx2Transport::addByte(0); + } } void Pxx2Pulses::setupBindFrame(uint8_t module) diff --git a/radio/src/pulses/pxx2.h b/radio/src/pulses/pxx2.h index dc5b341db..84241c779 100644 --- a/radio/src/pulses/pxx2.h +++ b/radio/src/pulses/pxx2.h @@ -21,10 +21,13 @@ #ifndef _PULSES_PXX2_H_ #define _PULSES_PXX2_H_ +#include "fifo.h" #include "./pxx.h" #define PXX2_TYPE_C_MODULE 0x01 #define PXX2_TYPE_ID_REGISTER 0x01 + #define REGISTER_COUNTER_ID_RECEIVED 1001 + #define REGISTER_COUNTER_PASSWORD_RECEIVED 1002 #define PXX2_TYPE_ID_BIND 0x02 #define PXX2_TYPE_ID_CHANNELS 0x03 #define PXX2_TYPE_ID_SPORT 0xFE @@ -35,6 +38,43 @@ #define PXX2_FLAG0_FAILSAFE (1 << 6) +class ModuleFifo : public Fifo { + public: + bool getFrame(uint8_t * frame) + { + while (1) { + if (isEmpty()) { + return false; + } + else if (fifo[ridx] != 0xFE) { + skip(); + } + else { + break; + } + } + + uint32_t next = nextIndex(ridx); + uint8_t len = fifo[next]; + if (size() < unsigned(len + 4 /* 2 bytes header + 2 bytes CRC */)) { + return false; + } + + for (uint32_t i=0; iSR; + + while (status & (USART_FLAG_RXNE | USART_FLAG_ERRORS)) { + uint8_t data = INTMODULE_USART->DR; + if (status & USART_FLAG_ERRORS) { + intmoduleErrors++; + } + else { + intmoduleFifo.push(data); + } + status = INTMODULE_USART->SR; } } @@ -110,3 +119,4 @@ void intmoduleSendNextFrame() USART_DMACmd(INTMODULE_USART, USART_DMAReq_Tx, ENABLE); } } + diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h index 2aabf23f1..d203c168f 100644 --- a/radio/src/targets/taranis/hal.h +++ b/radio/src/targets/taranis/hal.h @@ -599,24 +599,24 @@ #define INTMODULE_PWR_GPIO GPIOD #define INTMODULE_PWR_GPIO_PIN GPIO_Pin_9 // PD.09 #endif - #define INTMODULE_TX_GPIO GPIOB + #define INTMODULE_GPIO GPIOB #define INTMODULE_TX_GPIO_PIN GPIO_Pin_6 // PB.06 - #define INTMODULE_RX_GPIO GPIOB #define INTMODULE_RX_GPIO_PIN GPIO_Pin_7 // PB.07 - #define INTMODULE_TX_GPIO_PinSource GPIO_PinSource6 - #define INTMODULE_RX_GPIO_PinSource GPIO_PinSource7 + #define INTMODULE_GPIO_PinSource_TX GPIO_PinSource6 + #define INTMODULE_GPIO_PinSource_RX GPIO_PinSource7 #define INTMODULE_USART USART1 - #define INTMODULE_TX_GPIO_AF GPIO_AF_USART1 + #define INTMODULE_GPIO_AF GPIO_AF_USART1 + #define INTMODULE_USART_IRQHandler USART1_IRQHandler #define INTMODULE_USART_IRQn USART1_IRQn #define INTMODULE_DMA_STREAM DMA2_Stream7 - #define INTMODULE_DMA_STREAM_IRQ DMA2_Stream7_IRQn - #define INTMODULE_DMA_STREAM_IRQHandler DMA2_Stream7_IRQHandler + #define INTMODULE_DMA_STREAM_IRQ DMA2_Stream7_IRQn + #define INTMODULE_DMA_STREAM_IRQHandler DMA2_Stream7_IRQHandler #define INTMODULE_DMA_FLAG_TC DMA_IT_TCIF7 #define INTMODULE_DMA_CHANNEL DMA_Channel_4 - #define INTMODULE_TIMER TIM3 - #define INTMODULE_TIMER_IRQn TIM3_IRQn - #define INTMODULE_TIMER_IRQHandler TIM3_IRQHandler - #define INTMODULE_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) + // #define INTMODULE_TIMER TIM3 + // #define INTMODULE_TIMER_IRQn TIM3_IRQn + // #define INTMODULE_TIMER_IRQHandler TIM3_IRQHandler + // #define INTMODULE_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) #elif defined(PCBX9E) || defined(PCBX9DP) || defined(PCBX7) #define INTMODULE_PULSES #define INTMODULE_RCC_AHB1Periph (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_DMA2) diff --git a/radio/src/targets/taranis/telemetry_driver.cpp b/radio/src/targets/taranis/telemetry_driver.cpp index c805f56e8..fb854c482 100644 --- a/radio/src/targets/taranis/telemetry_driver.cpp +++ b/radio/src/targets/taranis/telemetry_driver.cpp @@ -37,12 +37,10 @@ void telemetryPortInit(uint32_t baudrate, uint8_t mode) NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); - USART_InitTypeDef USART_InitStructure; - GPIO_InitTypeDef GPIO_InitStructure; - - GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_RX, TELEMETRY_GPIO_AF); GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_TX, TELEMETRY_GPIO_AF); + GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_RX, TELEMETRY_GPIO_AF); + GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = TELEMETRY_TX_GPIO_PIN | TELEMETRY_RX_GPIO_PIN; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; @@ -56,6 +54,8 @@ void telemetryPortInit(uint32_t baudrate, uint8_t mode) GPIO_Init(TELEMETRY_DIR_GPIO, &GPIO_InitStructure); TELEMETRY_DIR_INPUT(); + USART_DeInit(TELEMETRY_USART); + USART_InitTypeDef USART_InitStructure; USART_InitStructure.USART_BaudRate = baudrate; if (mode & TELEMETRY_SERIAL_8E2) { USART_InitStructure.USART_WordLength = USART_WordLength_9b; @@ -70,8 +70,8 @@ void telemetryPortInit(uint32_t baudrate, uint8_t 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); NVIC_EnableIRQ(TELEMETRY_USART_IRQn); diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 8fabe7692..377be5f98 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -71,6 +71,35 @@ void processTelemetryData(uint8_t data) processFrskyTelemetryData(data); } +void processRegisterFrame(uint8_t module, uint8_t * frame) +{ + if (frame[3] == 0x00) { + // RX_ID follows, we discard it for now + moduleSettings[module].counter = REGISTER_COUNTER_ID_RECEIVED; + } +} + +void processRadioFrame(uint8_t module, uint8_t * frame) +{ + switch (frame[2]) { + case PXX2_TYPE_ID_REGISTER: + processRegisterFrame(module, frame); + break; + } +} + +void processModuleFrame(uint8_t module, uint8_t * frame) +{ + switch (frame[1]) { + case PXX2_TYPE_C_MODULE: + processRadioFrame(module, frame); + break; + + default: + break; + } +} + void telemetryWakeup() { uint8_t requiredTelemetryProtocol = modelTelemetryProtocol(); @@ -82,9 +111,16 @@ void telemetryWakeup() telemetryInit(requiredTelemetryProtocol); } #else - if (telemetryProtocol != requiredTelemetryProtocol) { - telemetryInit(requiredTelemetryProtocol); - } + if (telemetryProtocol != requiredTelemetryProtocol) { + telemetryInit(requiredTelemetryProtocol); + } +#endif + +#if defined(INTMODULE_USART) + uint8_t frame[32]; + if (intmoduleFifo.getFrame(frame)) { + processModuleFrame(INTERNAL_MODULE, frame); + } #endif #if defined(STM32)