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