radio/src/targets/horus/telemetry_driver.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23 Fifo<uint8_t, TELEMETRY_FIFO_SIZE> telemetryNoDMAFifo;
  24 uint32_t telemetryErrors = 0;
  25
  26 #if defined(PCBX12S)
  27 DMAFifo<TELEMETRY_FIFO_SIZE> telemetryDMAFifo __DMA (TELEMETRY_DMA_Stream_RX);
  28 uint8_t telemetryFifoMode;
  29 #endif
  30
  31 static void telemetryInitDirPin()
  32 {
  33   GPIO_InitTypeDef GPIO_InitStructure;
  34   GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
  35   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  36   GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
  37   GPIO_InitStructure.GPIO_Pin   = TELEMETRY_DIR_GPIO_PIN;
  38   GPIO_Init(TELEMETRY_DIR_GPIO, &GPIO_InitStructure);
  39   GPIO_ResetBits(TELEMETRY_DIR_GPIO, TELEMETRY_DIR_GPIO_PIN);
  40 }
  41
  42 void telemetryPortInit(uint32_t baudrate, uint8_t mode)
  43 {
  44   if (baudrate == 0) {
  45     USART_DeInit(TELEMETRY_USART);
  46     return;
  47   }
  48
  49   NVIC_InitTypeDef NVIC_InitStructure;
  50   NVIC_InitStructure.NVIC_IRQChannel = TELEMETRY_DMA_TX_Stream_IRQ;
  51   NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
  52   NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; /* Not used as 4 bits are used for the pre-emption priority. */;
  53   NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  54   NVIC_Init(&NVIC_InitStructure);
  55
  56   USART_InitTypeDef USART_InitStructure;
  57   GPIO_InitTypeDef GPIO_InitStructure;
  58
  59   GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_RX, TELEMETRY_GPIO_AF);
  60   GPIO_PinAFConfig(TELEMETRY_GPIO, TELEMETRY_GPIO_PinSource_TX, TELEMETRY_GPIO_AF);
  61
  62   GPIO_InitStructure.GPIO_Pin = TELEMETRY_TX_GPIO_PIN | TELEMETRY_RX_GPIO_PIN;
  63   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  64   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  65   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
  66   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  67   GPIO_Init(TELEMETRY_GPIO, &GPIO_InitStructure);
  68
  69   telemetryInitDirPin();
  70
  71   USART_InitStructure.USART_BaudRate = baudrate;
  72   if (mode & TELEMETRY_SERIAL_8E2) {
  73     USART_InitStructure.USART_WordLength = USART_WordLength_9b;
  74     USART_InitStructure.USART_StopBits = USART_StopBits_2;
  75     USART_InitStructure.USART_Parity = USART_Parity_Even;
  76   }
  77   else {
  78     USART_InitStructure.USART_WordLength = USART_WordLength_8b;
  79     USART_InitStructure.USART_StopBits = USART_StopBits_1;
  80     USART_InitStructure.USART_Parity = USART_Parity_No;
  81   }
  82   USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  83   USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
  84   USART_Init(TELEMETRY_USART, &USART_InitStructure);
  85
  86 #if defined(PCBX12S)
  87   telemetryFifoMode = mode;
  88
  89   DMA_Cmd(TELEMETRY_DMA_Stream_RX, DISABLE);
  90   USART_DMACmd(TELEMETRY_USART, USART_DMAReq_Rx, DISABLE);
  91   DMA_DeInit(TELEMETRY_DMA_Stream_RX);
  92
  93   if (mode & TELEMETRY_SERIAL_WITHOUT_DMA) {
  94     USART_Cmd(TELEMETRY_USART, ENABLE);
  95     USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, ENABLE);
  96     NVIC_SetPriority(TELEMETRY_USART_IRQn, 6);
  97     NVIC_EnableIRQ(TELEMETRY_USART_IRQn);
  98   }
  99   else {
 100     DMA_InitTypeDef DMA_InitStructure;
 101     telemetryDMAFifo.clear();
 102
 103     USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, DISABLE);
 104     USART_ITConfig(TELEMETRY_USART, USART_IT_TXE, DISABLE);
 105     NVIC_SetPriority(TELEMETRY_USART_IRQn, 6);
 106     NVIC_EnableIRQ(TELEMETRY_USART_IRQn);
 107
 108     DMA_InitStructure.DMA_Channel = TELEMETRY_DMA_Channel_RX;
 109     DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&TELEMETRY_USART->DR);
 110     DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(telemetryDMAFifo.buffer());
 111     DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
 112     DMA_InitStructure.DMA_BufferSize = telemetryDMAFifo.size();
 113     DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 114     DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
 115     DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
 116     DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
 117     DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
 118     DMA_InitStructure.DMA_Priority = DMA_Priority_Low;
 119     DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
 120     DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
 121     DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 122     DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 123     DMA_Init(TELEMETRY_DMA_Stream_RX, &DMA_InitStructure);
 124     USART_DMACmd(TELEMETRY_USART, USART_DMAReq_Rx, ENABLE);
 125     USART_Cmd(TELEMETRY_USART, ENABLE);
 126     DMA_Cmd(TELEMETRY_DMA_Stream_RX, ENABLE);
 127   }
 128 #else
 129   USART_Cmd(TELEMETRY_USART, ENABLE);
 130   USART_ITConfig(TELEMETRY_USART, USART_IT_RXNE, ENABLE);
 131   NVIC_SetPriority(TELEMETRY_USART_IRQn, 6);
 132   NVIC_EnableIRQ(TELEMETRY_USART_IRQn);
 133 #endif
 134 }
 135
 136 // soft serial vars
 137 static uint8_t rxBitCount;
 138 static uint8_t rxByte;
 139
 140 void telemetryPortInvertedInit(uint32_t baudrate)
 141 {
 142   if (baudrate == 0) {
 143     NVIC_DisableIRQ(TELEMETRY_EXTI_IRQn);
 144     NVIC_DisableIRQ(TELEMETRY_TIMER_IRQn);
 145
 146     EXTI_InitTypeDef EXTI_InitStructure;
 147     EXTI_StructInit(&EXTI_InitStructure);
 148     EXTI_InitStructure.EXTI_Line = TELEMETRY_EXTI_LINE;
 149     EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
 150     EXTI_InitStructure.EXTI_Trigger = TELEMETRY_EXTI_TRIGGER;
 151     EXTI_InitStructure.EXTI_LineCmd = DISABLE;
 152     EXTI_Init(&EXTI_InitStructure);
 153     return;
 154   }
 155
 156   rxBitCount = 0;
 157
 158   // configure bit sample timer
 159   RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
 160   TELEMETRY_TIMER->PSC = (PERI2_FREQUENCY * TIMER_MULT_APB2) / 2000000 - 1; // 0.5uS
 161   TELEMETRY_TIMER->CCER = 0;
 162   TELEMETRY_TIMER->CCMR1 = 0;
 163   TELEMETRY_TIMER->CR1 = TIM_CR1_CEN;
 164   TELEMETRY_TIMER->DIER = TIM_DIER_UIE;
 165
 166   NVIC_SetPriority(TELEMETRY_TIMER_IRQn, 0);
 167   NVIC_EnableIRQ(TELEMETRY_TIMER_IRQn);
 168
 169   // init TELEMETRY_RX_GPIO_PIN
 170   GPIO_InitTypeDef GPIO_InitStructure;
 171   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
 172   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
 173   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
 174   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
 175   GPIO_InitStructure.GPIO_Pin = TELEMETRY_RX_GPIO_PIN;
 176   GPIO_Init(TELEMETRY_GPIO, &GPIO_InitStructure);
 177
 178   telemetryInitDirPin();
 179
 180   // Connect EXTI line to TELEMETRY RX pin
 181   SYSCFG_EXTILineConfig(TELEMETRY_EXTI_PortSource, TELEMETRY_EXTI_PinSource);
 182
 183   // Configure EXTI for raising edge (start bit)
 184   EXTI_InitTypeDef EXTI_InitStructure;
 185   EXTI_StructInit(&EXTI_InitStructure);
 186   EXTI_InitStructure.EXTI_Line = TELEMETRY_EXTI_LINE;
 187   EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
 188   EXTI_InitStructure.EXTI_Trigger = TELEMETRY_EXTI_TRIGGER;
 189   EXTI_InitStructure.EXTI_LineCmd = ENABLE;
 190   EXTI_Init(&EXTI_InitStructure);
 191
 192   NVIC_SetPriority(TELEMETRY_EXTI_IRQn, 0);
 193   NVIC_EnableIRQ(TELEMETRY_EXTI_IRQn);
 194 }
 195
 196 void telemetryPortInvertedRxBit()
 197 {
 198   if (rxBitCount < 8) {
 199     if (rxBitCount == 0) {
 200       TELEMETRY_TIMER->ARR = 34;
 201       rxByte = 0;
 202     }
 203     else {
 204       rxByte >>= 1;
 205     }
 206
 207     if (GPIO_ReadInputDataBit(TELEMETRY_GPIO, TELEMETRY_RX_GPIO_PIN) == Bit_RESET)
 208       rxByte |= 0x80;
 209
 210     ++rxBitCount;
 211   }
 212   else if (rxBitCount == 8) {
 213
 214     telemetryNoDMAFifo.push(rxByte);
 215     rxBitCount = 0;
 216
 217     // disable timer
 218     TELEMETRY_TIMER->CR1 &= ~TIM_CR1_CEN;
 219
 220     // re-enable start bit interrupt
 221     EXTI->IMR |= EXTI_IMR_MR6;
 222   }
 223 }
 224
 225 void telemetryPortSetDirectionOutput()
 226 {
 227   TELEMETRY_DIR_GPIO->BSRRL = TELEMETRY_DIR_GPIO_PIN;     // output enable
 228   TELEMETRY_USART->CR1 &= ~USART_CR1_RE;                  // turn off receiver
 229 }
 230
 231 void sportWaitTransmissionComplete()
 232 {
 233   while (!(TELEMETRY_USART->SR & USART_SR_TC));
 234 }
 235
 236 void telemetryPortSetDirectionInput()
 237 {
 238   sportWaitTransmissionComplete();
 239   TELEMETRY_DIR_GPIO->BSRRH = TELEMETRY_DIR_GPIO_PIN;     // output disable
 240   TELEMETRY_USART->CR1 |= USART_CR1_RE;                   // turn on receiver
 241 }
 242
 243 void sportSendByte(uint8_t byte)
 244 {
 245   telemetryPortSetDirectionOutput();
 246
 247   while (!(TELEMETRY_USART->SR & USART_SR_TXE));
 248   USART_SendData(TELEMETRY_USART, byte);
 249 }
 250
 251 void sportSendByteLoop(uint8_t byte)
 252 {
 253   telemetryPortSetDirectionOutput();
 254
 255   outputTelemetryBuffer.data[0] = byte;
 256
 257   DMA_InitTypeDef DMA_InitStructure;
 258   DMA_DeInit(TELEMETRY_DMA_Stream_TX);
 259   DMA_InitStructure.DMA_Channel = TELEMETRY_DMA_Channel_TX;
 260   DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&TELEMETRY_USART->DR);
 261   DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
 262   DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(outputTelemetryBuffer.data);
 263   DMA_InitStructure.DMA_BufferSize = 1;
 264   DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 265   DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Disable;
 266   DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
 267   DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
 268   DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
 269   DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
 270   DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
 271   DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
 272   DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 273   DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 274   DMA_Init(TELEMETRY_DMA_Stream_TX, &DMA_InitStructure);
 275   DMA_Cmd(TELEMETRY_DMA_Stream_TX, ENABLE);
 276   USART_DMACmd(TELEMETRY_USART, USART_DMAReq_Tx, ENABLE);
 277 }
 278
 279 void sportSendBuffer(const uint8_t * buffer, uint32_t count)
 280 {
 281   telemetryPortSetDirectionOutput();
 282
 283   DMA_InitTypeDef DMA_InitStructure;
 284   DMA_DeInit(TELEMETRY_DMA_Stream_TX);
 285   DMA_InitStructure.DMA_Channel = TELEMETRY_DMA_Channel_TX;
 286   DMA_InitStructure.DMA_PeripheralBaseAddr = CONVERT_PTR_UINT(&TELEMETRY_USART->DR);
 287   DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
 288   DMA_InitStructure.DMA_Memory0BaseAddr = CONVERT_PTR_UINT(buffer);
 289   DMA_InitStructure.DMA_BufferSize = count;
 290   DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
 291   DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
 292   DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
 293   DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
 294   DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
 295   DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
 296   DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
 297   DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
 298   DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
 299   DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
 300   DMA_Init(TELEMETRY_DMA_Stream_TX, &DMA_InitStructure);
 301   DMA_Cmd(TELEMETRY_DMA_Stream_TX, ENABLE);
 302   USART_DMACmd(TELEMETRY_USART, USART_DMAReq_Tx, ENABLE);
 303   DMA_ITConfig(TELEMETRY_DMA_Stream_TX, DMA_IT_TC, ENABLE);
 304
 305   // enable interrupt and set it's priority
 306   NVIC_EnableIRQ(TELEMETRY_DMA_TX_Stream_IRQ) ;
 307   NVIC_SetPriority(TELEMETRY_DMA_TX_Stream_IRQ, 7);
 308 }
 309
 310 extern "C" void TELEMETRY_DMA_TX_IRQHandler(void)
 311 {
 312   DEBUG_INTERRUPT(INT_TELEM_DMA);
 313   if (DMA_GetITStatus(TELEMETRY_DMA_Stream_TX, TELEMETRY_DMA_TX_FLAG_TC)) {
 314     DMA_ClearITPendingBit(TELEMETRY_DMA_Stream_TX, TELEMETRY_DMA_TX_FLAG_TC);
 315     TELEMETRY_USART->CR1 |= USART_CR1_TCIE;
 316     if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT) {
 317       outputTelemetryBuffer.reset();
 318     }
 319   }
 320 }
 321
 322 #define USART_FLAG_ERRORS (USART_FLAG_ORE | USART_FLAG_NE | USART_FLAG_FE | USART_FLAG_PE)
 323 extern "C" void TELEMETRY_USART_IRQHandler(void)
 324 {
 325   DEBUG_INTERRUPT(INT_TELEM_USART);
 326   uint32_t status = TELEMETRY_USART->SR;
 327
 328   if ((status & USART_SR_TC) && (TELEMETRY_USART->CR1 & USART_CR1_TCIE)) {
 329     TELEMETRY_USART->CR1 &= ~USART_CR1_TCIE;
 330     telemetryPortSetDirectionInput();
 331     while (status & (USART_FLAG_RXNE)) {
 332       status = TELEMETRY_USART->DR;
 333       status = TELEMETRY_USART->SR;
 334     }
 335   }
 336
 337   while (status & (USART_FLAG_RXNE | USART_FLAG_ERRORS)) {
 338     uint8_t data = TELEMETRY_USART->DR;
 339     if (status & USART_FLAG_ERRORS) {
 340       telemetryErrors++;
 341     }
 342     else {
 343       telemetryNoDMAFifo.push(data);
 344 #if defined(LUA)
 345       if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT) {
 346         static uint8_t prevdata;
 347         if (prevdata == 0x7E && outputTelemetryBuffer.size > 0 && outputTelemetryBuffer.destination == TELEMETRY_ENDPOINT_SPORT && data == outputTelemetryBuffer.sport.physicalId) {
 348           sportSendBuffer(outputTelemetryBuffer.data + 1, outputTelemetryBuffer.size - 1);
 349         }
 350         prevdata = data;
 351       }
 352 #endif
 353     }
 354     status = TELEMETRY_USART->SR;
 355   }
 356 }
 357
 358 extern "C" void TELEMETRY_EXTI_IRQHandler(void)
 359 {
 360   if (EXTI_GetITStatus(TELEMETRY_EXTI_LINE) != RESET) {
 361
 362     if (rxBitCount == 0) {
 363
 364       TELEMETRY_TIMER->ARR = 48; // 1,5 cycle from start at 57600bps
 365       TELEMETRY_TIMER->CR1 |= TIM_CR1_CEN;
 366
 367       // disable start bit interrupt
 368       EXTI->IMR &= ~EXTI_IMR_MR6;
 369     }
 370
 371     EXTI_ClearITPendingBit(TELEMETRY_EXTI_LINE);
 372   }
 373 }
 374
 375 extern "C" void TELEMETRY_TIMER_IRQHandler()
 376 {
 377   TELEMETRY_TIMER->SR &= ~TIM_SR_UIF;
 378   telemetryPortInvertedRxBit();
 379 }
 380
 381 // TODO we should have telemetry in an higher layer, functions above should move to a sport_driver.cpp
 382 bool telemetryGetByte(uint8_t * byte)
 383 {
 384 #if defined(PCBX12S)
 385   if (telemetryFifoMode & TELEMETRY_SERIAL_WITHOUT_DMA)
 386     return telemetryNoDMAFifo.pop(*byte);
 387   else
 388     return telemetryDMAFifo.pop(*byte);
 389 #else
 390   return telemetryNoDMAFifo.pop(*byte);
 391 #endif
 392 }
 393
 394 void telemetryClearFifo()
 395 {
 396 #if defined(PCBX12S)
 397   telemetryDMAFifo.clear();
 398 #endif
 399
 400   telemetryNoDMAFifo.clear();
 401 }
 402