REGISTER should be ready for tests

radio/src/fifo.h

@@ -21,6 +21,8 @@
 #ifndef _FIFO_H_
 #define _FIFO_H_
 
+#include <inttypes.h>
+
 template <class T, int N>
 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);
     }
 
@@ -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_

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<NUM_MODULES; i++) {
-    moduleSettings[i].failsafeCounter = 100;
+    moduleSettings[i].counter = 100;
   }
 }
 

radio/src/pulses/pxx1.cpp

@@ -33,14 +33,14 @@ uint8_t Pxx1Pulses<PxxTransport>::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) {
+    if (moduleSettings[module].counter-- == 0) {
-      // failsafeCounter is also used for knowing if the frame is odd / even
+      // counter is also used for knowing if the frame is odd / even
-      moduleSettings[module].failsafeCounter = 1000;
+      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<PxxTransport>::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);

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) {
+    if (moduleSettings[module].counter-- == 0) {
-      moduleSettings[module].failsafeCounter = 1000;
+      moduleSettings[module].counter = 1000;
       flag0 |= PXX2_FLAG0_FAILSAFE;
     }
   }
@@ -57,9 +57,21 @@ 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);
+
+  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)
 {

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<uint8_t, 32> {
+  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; i<len; i++) {
+        next = nextIndex(next);
+        frame[i] = fifo[next];
+      }
+
+      // TODO CRC CHECK
+      next = nextIndex(next);
+      next = nextIndex(next);
+
+      ridx = nextIndex(next);
+      return true;
+    }
+};
+
+extern ModuleFifo intmoduleFifo;
 
 // should not be used anymore
 class SportCrcMixin {

radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp

@@ -20,6 +20,9 @@
 
 #include "opentx.h"
 
+ModuleFifo intmoduleFifo;
+uint8_t intmoduleErrors;
+
 void intmoduleStop()
 {
   INTERNAL_MODULE_OFF();
@@ -47,23 +50,17 @@ void intmodulePxx2Start()
   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStructure);
 
-  // TX Pin
+  GPIO_PinAFConfig(INTMODULE_GPIO, INTMODULE_GPIO_PinSource_TX, INTMODULE_GPIO_AF);
-  GPIO_PinAFConfig(INTMODULE_TX_GPIO, INTMODULE_TX_GPIO_PinSource, INTMODULE_TX_GPIO_AF);
+  GPIO_PinAFConfig(INTMODULE_GPIO, INTMODULE_GPIO_PinSource_RX, INTMODULE_GPIO_AF);
 
   GPIO_InitTypeDef GPIO_InitStructure;
-  GPIO_InitStructure.GPIO_Pin = INTMODULE_TX_GPIO_PIN;
+  GPIO_InitStructure.GPIO_Pin = INTMODULE_TX_GPIO_PIN | INTMODULE_RX_GPIO_PIN;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
-  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
-  GPIO_Init(INTMODULE_TX_GPIO, &GPIO_InitStructure);
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_Init(INTMODULE_GPIO, &GPIO_InitStructure);
 
-  // RX Pin
-  GPIO_PinAFConfig(INTMODULE_RX_GPIO, INTMODULE_RX_GPIO_PinSource, INTMODULE_TX_GPIO_AF);
-  GPIO_InitStructure.GPIO_Pin = INTMODULE_RX_GPIO_PIN;
-  GPIO_Init(INTMODULE_RX_GPIO, &GPIO_InitStructure);
-
-  // UART config
   USART_DeInit(INTMODULE_USART);
   USART_InitTypeDef USART_InitStructure;
   USART_InitStructure.USART_BaudRate = INTMODULE_USART_PXX_BAUDRATE;
@@ -71,17 +68,29 @@ void intmodulePxx2Start()
   USART_InitStructure.USART_StopBits = USART_StopBits_1;
   USART_InitStructure.USART_WordLength = USART_WordLength_8b;
   USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
-  USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
+  USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
   USART_Init(INTMODULE_USART, &USART_InitStructure);
   USART_Cmd(INTMODULE_USART, ENABLE);
+
+  USART_ITConfig(INTMODULE_USART, USART_IT_RXNE, ENABLE);
+  NVIC_SetPriority(INTMODULE_USART_IRQn, 6);
+  NVIC_EnableIRQ(INTMODULE_USART_IRQn);
 }
 
-extern "C" void INTMODULE_DMA_STREAM_IRQHandler(void)
+#define USART_FLAG_ERRORS (USART_FLAG_ORE | USART_FLAG_NE | USART_FLAG_FE | USART_FLAG_PE)
+extern "C" void INTMODULE_USART_IRQHandler(void)
 {
-  DEBUG_INTERRUPT(INT_DMA2S7);
+  uint32_t status = INTMODULE_USART->SR;
-  if (DMA_GetITStatus(INTMODULE_DMA_STREAM, INTMODULE_DMA_FLAG_TC)) {
+
-    // TODO we could send the 8 next channels here (when needed)
+  while (status & (USART_FLAG_RXNE | USART_FLAG_ERRORS)) {
-    DMA_ClearITPendingBit(INTMODULE_DMA_STREAM, INTMODULE_DMA_FLAG_TC);
+    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);
   }
 }
+

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_GPIO_PinSource_TX   GPIO_PinSource6
-  #define INTMODULE_RX_GPIO_PinSource   GPIO_PinSource7
+  #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_FLAG_TC         DMA_IT_TCIF7
   #define INTMODULE_DMA_CHANNEL         DMA_Channel_4
-  #define INTMODULE_TIMER               TIM3
+  // #define INTMODULE_TIMER               TIM3
-  #define INTMODULE_TIMER_IRQn          TIM3_IRQn
+  // #define INTMODULE_TIMER_IRQn          TIM3_IRQn
-  #define INTMODULE_TIMER_IRQHandler    TIM3_IRQHandler
+  // #define INTMODULE_TIMER_IRQHandler    TIM3_IRQHandler
-  #define INTMODULE_TIMER_FREQ          (PERI1_FREQUENCY * TIMER_MULT_APB1)
+  // #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)

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);

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();
@@ -87,6 +116,13 @@ void telemetryWakeup()
   }
 #endif
 
+#if defined(INTMODULE_USART)
+  uint8_t frame[32];
+  if (intmoduleFifo.getFrame(frame)) {
+    processModuleFrame(INTERNAL_MODULE, frame);
+  }
+#endif
+
 #if defined(STM32)
   uint8_t data;
   if (telemetryGetByte(&data)) {