radio/src/targets/common/arm/stm32/sticks_pwm_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 bool sticks_pwm_disabled = false;
  24 volatile uint32_t pwm_interrupt_count;
  25 volatile uint16_t timer_capture_values[NUM_PWMSTICKS];
  26
  27 void sticksPwmInit()
  28 {
  29   GPIO_InitTypeDef GPIO_InitStructure;
  30   GPIO_InitStructure.GPIO_Pin = PWM_GPIOA_PINS;
  31   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
  32   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  33   GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  34   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
  35   GPIO_Init(PWM_GPIO, &GPIO_InitStructure);
  36
  37   GPIO_PinAFConfig(PWM_GPIO, GPIO_PinSource0, PWM_GPIO_AF);
  38   GPIO_PinAFConfig(PWM_GPIO, GPIO_PinSource1, PWM_GPIO_AF);
  39   GPIO_PinAFConfig(PWM_GPIO, GPIO_PinSource2, PWM_GPIO_AF);
  40   GPIO_PinAFConfig(PWM_GPIO, GPIO_PinSource3, PWM_GPIO_AF);
  41
  42   PWM_TIMER->CR1 &= ~TIM_CR1_CEN; // Stop timer
  43   PWM_TIMER->PSC = 80;
  44   PWM_TIMER->ARR = 0xffff;
  45   PWM_TIMER->CCMR1 = TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0;
  46   PWM_TIMER->CCMR2 = TIM_CCMR2_CC3S_0 | TIM_CCMR2_CC4S_0;
  47   PWM_TIMER->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC3E | TIM_CCER_CC4E;
  48   PWM_TIMER->DIER |= TIM_IT_CC1|TIM_IT_CC2|TIM_IT_CC3|TIM_IT_CC4;
  49   PWM_TIMER->CR1 = TIM_CR1_CEN; // Start timer
  50
  51   NVIC_EnableIRQ(PWM_IRQn);
  52   NVIC_SetPriority(PWM_IRQn, 10);
  53 }
  54
  55 inline uint32_t TIM_GetCapture_Stick(uint8_t n)
  56 {
  57   switch (n) {
  58     case 0:
  59       return PWM_TIMER->CCR1;
  60     case 1:
  61       return PWM_TIMER->CCR2;
  62     case 2:
  63       return PWM_TIMER->CCR3;
  64     case 3:
  65       return PWM_TIMER->CCR4;
  66     default:
  67       return 0;
  68   }
  69 }
  70
  71 inline void TIM_SetPolarityRising(uint8_t n)
  72 {
  73   PWM_TIMER->CCER &= ~(TIM_CCER_CC1P << (n * 4));
  74 }
  75
  76 inline void TIM_SetPolarityFalling(uint8_t n)
  77 {
  78   PWM_TIMER->CCER |= (TIM_CCER_CC1P << (n * 4));
  79 }
  80
  81 inline void TIM_ClearITPendingBit(uint8_t n)
  82 {
  83   PWM_TIMER->SR = ~(TIM_IT_CC1 << n);
  84 }
  85
  86 inline uint32_t diff_with_16bits_overflow(uint32_t a, uint32_t b)
  87 {
  88   if (b > a)
  89     return b - a;
  90   else
  91     return b + 0xffff - a;
  92 }
  93
  94 extern "C" void PWM_IRQHandler(void)
  95 {
  96   static uint8_t  timer_capture_states[NUM_PWMSTICKS];
  97   static uint32_t timer_capture_rising_time[NUM_PWMSTICKS];
  98
  99   for (uint8_t i=0; i<NUM_PWMSTICKS; i++) {
 100     if (PWM_TIMER->SR & (TIM_DIER_CC1IE << i)) {
 101       uint32_t capture = TIM_GetCapture_Stick(i);
 102       pwm_interrupt_count++; // overflow may happen but we only use this to detect PWM / ADC on radio startup
 103       if (timer_capture_states[i] != 0) {
 104         uint32_t value = diff_with_16bits_overflow(timer_capture_rising_time[i], capture);
 105         if (value < 10000) {
 106           timer_capture_values[i] = (uint16_t) value;
 107         }
 108         TIM_SetPolarityRising(i);
 109         timer_capture_states[i] = 0;
 110       }
 111       else {
 112         timer_capture_rising_time[i] = capture;
 113         TIM_SetPolarityFalling(i);
 114         timer_capture_states[i] = 0x80;
 115       }
 116       TIM_ClearITPendingBit(i);
 117     }
 118   }
 119 }
 120
 121 void sticksPwmRead(uint16_t * values)
 122 {
 123   values[0] = timer_capture_values[0];
 124   values[1] = timer_capture_values[1];
 125   values[2] = timer_capture_values[3];
 126   values[3] = timer_capture_values[2];
 127 }