src/main/drivers/pwm_output.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include <stdlib.h>
  22
  23 #include "platform.h"
  24
  25 #include "gpio.h"
  26 #include "io.h"
  27 #include "io_impl.h"
  28 #include "timer.h"
  29 #include "pwm_mapping.h"
  30 #include "pwm_output.h"
  31
  32 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
  33
  34 typedef struct {
  35     volatile timCCR_t *ccr;
  36     TIM_TypeDef *tim;
  37     uint16_t period;
  38     pwmWriteFuncPtr pwmWritePtr;
  39 } pwmOutputPort_t;
  40
  41 static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
  42
  43 static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
  44
  45 #ifdef USE_SERVOS
  46 static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
  47 #endif
  48
  49 static uint8_t allocatedOutputPortCount = 0;
  50
  51 static bool pwmMotorsEnabled = true;
  52
  53
  54 static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
  55 {
  56     TIM_OCInitTypeDef  TIM_OCInitStructure;
  57
  58     TIM_OCStructInit(&TIM_OCInitStructure);
  59     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
  60     if (output & TIMER_OUTPUT_N_CHANNEL) {
  61         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
  62         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
  63     } else {
  64         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  65         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
  66     }
  67     TIM_OCInitStructure.TIM_Pulse = value;
  68     TIM_OCInitStructure.TIM_OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_High : TIM_OCPolarity_Low;
  69     TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
  70
  71     switch (channel) {
  72     case TIM_Channel_1:
  73         TIM_OC1Init(tim, &TIM_OCInitStructure);
  74         TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable);
  75         break;
  76     case TIM_Channel_2:
  77         TIM_OC2Init(tim, &TIM_OCInitStructure);
  78         TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable);
  79         break;
  80     case TIM_Channel_3:
  81         TIM_OC3Init(tim, &TIM_OCInitStructure);
  82         TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable);
  83         break;
  84     case TIM_Channel_4:
  85         TIM_OC4Init(tim, &TIM_OCInitStructure);
  86         TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
  87         break;
  88     }
  89 }
  90
  91 static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
  92 {
  93     gpio_config_t cfg;
  94
  95     cfg.pin = pin;
  96     cfg.mode = mode;
  97     cfg.speed = Speed_2MHz;
  98     gpioInit(gpio, &cfg);
  99 }
 100
 101 static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
 102 {
 103     pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
 104
 105     configTimeBase(timerHardware->tim, period, mhz);
 106     pwmGPIOConfig(IO_GPIOBYTAG(timerHardware->tag), IO_PINBYTAG(timerHardware->tag), Mode_AF_PP);
 107
 108     pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output & TIMER_OUTPUT_INVERTED);
 109     if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
 110         TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
 111     }
 112     TIM_Cmd(timerHardware->tim, ENABLE);
 113
 114     switch (timerHardware->channel) {
 115     case TIM_Channel_1:
 116         p->ccr = &timerHardware->tim->CCR1;
 117         break;
 118     case TIM_Channel_2:
 119         p->ccr = &timerHardware->tim->CCR2;
 120         break;
 121     case TIM_Channel_3:
 122         p->ccr = &timerHardware->tim->CCR3;
 123         break;
 124     case TIM_Channel_4:
 125         p->ccr = &timerHardware->tim->CCR4;
 126         break;
 127     }
 128     p->period = period;
 129     p->tim = timerHardware->tim;
 130
 131     *p->ccr = 0;
 132
 133     return p;
 134 }
 135
 136 static void pwmWriteBrushed(uint8_t index, uint16_t value)
 137 {
 138     *motors[index]->ccr = (value - 1000) * motors[index]->period / 1000;
 139 }
 140
 141 static void pwmWriteStandard(uint8_t index, uint16_t value)
 142 {
 143     *motors[index]->ccr = value;
 144 }
 145
 146 void pwmWriteMotor(uint8_t index, uint16_t value)
 147 {
 148     if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) {
 149         motors[index]->pwmWritePtr(index, value);
 150     }
 151 }
 152
 153 void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
 154 {
 155     for (int index = 0; index < motorCount; index++) {
 156         // Set the compare register to 0, which stops the output pulsing if the timer overflows
 157         *motors[index]->ccr = 0;
 158     }
 159 }
 160
 161 void pwmDisableMotors(void)
 162 {
 163     pwmMotorsEnabled = false;
 164 }
 165
 166 void pwmEnableMotors(void)
 167 {
 168     pwmMotorsEnabled = true;
 169 }
 170
 171 void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
 172 {
 173     TIM_TypeDef *lastTimerPtr = NULL;
 174
 175     for (int index = 0; index < motorCount; index++) {
 176
 177         // Force the timer to overflow if it's the first motor to output, or if we change timers
 178         if (motors[index]->tim != lastTimerPtr) {
 179             lastTimerPtr = motors[index]->tim;
 180             timerForceOverflow(motors[index]->tim);
 181         }
 182
 183         // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
 184         // This compare register will be set to the output value on the next main loop.
 185         *motors[index]->ccr = 0;
 186     }
 187 }
 188
 189 bool isMotorBrushed(uint16_t motorPwmRate)
 190 {
 191     return (motorPwmRate > 500);
 192 }
 193
 194 void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
 195 {
 196     const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
 197     motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse);
 198     motors[motorIndex]->pwmWritePtr = pwmWriteBrushed;
 199 }
 200
 201 void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
 202 {
 203     const uint32_t hz = PWM_TIMER_MHZ * 1000000;
 204     motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse);
 205     motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
 206 }
 207
 208 void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex)
 209 {
 210     motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, 0);
 211     motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
 212 }
 213
 214 #ifdef USE_SERVOS
 215 void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse)
 216 {
 217     servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse);
 218 }
 219
 220 void pwmWriteServo(uint8_t index, uint16_t value)
 221 {
 222     if (servos[index] && index < MAX_SERVOS) {
 223         *servos[index]->ccr = value;
 224     }
 225 }
 226 #endif