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 "timer.h"
  27
  28 #include "flight/failsafe.h" // FIXME dependency into the main code from a driver
  29
  30 #include "pwm_mapping.h"
  31
  32 #include "pwm_output.h"
  33
  34 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
  35
  36 typedef struct {
  37     volatile timCCR_t *ccr;
  38     TIM_TypeDef *tim;
  39     uint16_t period;
  40     pwmWriteFuncPtr pwmWritePtr;
  41 } pwmOutputPort_t;
  42
  43 static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
  44
  45 static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
  46
  47 #ifdef USE_SERVOS
  48 static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
  49 #endif
  50
  51 static uint8_t allocatedOutputPortCount = 0;
  52
  53 static bool pwmMotorsEnabled = true;
  54 static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
  55 {
  56     TIM_OCInitTypeDef  TIM_OCInitStructure;
  57
  58     TIM_OCStructInit(&TIM_OCInitStructure);
  59     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
  60     TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  61     TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
  62     TIM_OCInitStructure.TIM_Pulse = value;
  63     TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low;
  64     TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
  65
  66     switch (channel) {
  67         case TIM_Channel_1:
  68             TIM_OC1Init(tim, &TIM_OCInitStructure);
  69             TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable);
  70             break;
  71         case TIM_Channel_2:
  72             TIM_OC2Init(tim, &TIM_OCInitStructure);
  73             TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable);
  74             break;
  75         case TIM_Channel_3:
  76             TIM_OC3Init(tim, &TIM_OCInitStructure);
  77             TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable);
  78             break;
  79         case TIM_Channel_4:
  80             TIM_OC4Init(tim, &TIM_OCInitStructure);
  81             TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable);
  82             break;
  83     }
  84 }
  85
  86 static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
  87 {
  88     gpio_config_t cfg;
  89
  90     cfg.pin = pin;
  91     cfg.mode = mode;
  92     cfg.speed = Speed_2MHz;
  93     gpioInit(gpio, &cfg);
  94 }
  95
  96 static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
  97 {
  98     pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
  99
 100     configTimeBase(timerHardware->tim, period, mhz);
 101     pwmGPIOConfig(timerHardware->gpio, timerHardware->pin, Mode_AF_PP);
 102
 103
 104     pwmOCConfig(timerHardware->tim, timerHardware->channel, value);
 105     if (timerHardware->outputEnable)
 106         TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
 107     TIM_Cmd(timerHardware->tim, ENABLE);
 108
 109     switch (timerHardware->channel) {
 110         case TIM_Channel_1:
 111             p->ccr = &timerHardware->tim->CCR1;
 112             break;
 113         case TIM_Channel_2:
 114             p->ccr = &timerHardware->tim->CCR2;
 115             break;
 116         case TIM_Channel_3:
 117             p->ccr = &timerHardware->tim->CCR3;
 118             break;
 119         case TIM_Channel_4:
 120             p->ccr = &timerHardware->tim->CCR4;
 121             break;
 122     }
 123     p->period = period;
 124     p->tim = timerHardware->tim;
 125
 126     return p;
 127 }
 128
 129 static void pwmWriteBrushed(uint8_t index, uint16_t value)
 130 {
 131     *motors[index]->ccr = (value - 1000) * motors[index]->period / 1000;
 132 }
 133
 134 static void pwmWriteStandard(uint8_t index, uint16_t value)
 135 {
 136     *motors[index]->ccr = value;
 137 }
 138
 139 static void pwmWriteMultiShot(uint8_t index, uint16_t value)
 140 {
 141     *motors[index]->ccr = 60001 * (value - 1000) / 250000 + 60;
 142 }
 143
 144 void pwmWriteMotor(uint8_t index, uint16_t value)
 145 {
 146     if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled)
 147         motors[index]->pwmWritePtr(index, value);
 148 }
 149
 150 void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
 151 {
 152     uint8_t index;
 153
 154     for(index = 0; index < motorCount; index++){
 155         // Set the compare register to 0, which stops the output pulsing if the timer overflows
 156         *motors[index]->ccr = 0;
 157     }
 158 }
 159
 160 void pwmDisableMotors(void)
 161 {
 162     pwmMotorsEnabled = false;
 163 }
 164
 165 void pwmEnableMotors(void)
 166 {
 167     pwmMotorsEnabled = true;
 168 }
 169
 170 void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
 171 {
 172     uint8_t index;
 173     TIM_TypeDef *lastTimerPtr = NULL;
 174
 175     for(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
 181             timerForceOverflow(motors[index]->tim);
 182         }
 183
 184         // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
 185         // This compare register will be set to the output value on the next main loop.
 186         *motors[index]->ccr = 0;
 187     }
 188 }
 189
 190 bool isMotorBrushed(uint16_t motorPwmRate)
 191 {
 192     return (motorPwmRate > 500);
 193 }
 194
 195 void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
 196 {
 197     uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
 198     motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse);
 199     motors[motorIndex]->pwmWritePtr = pwmWriteBrushed;
 200 }
 201
 202 void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
 203 {
 204     uint32_t hz = PWM_TIMER_MHZ * 1000000;
 205     motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse);
 206     motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
 207 }
 208
 209 void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint8_t useUnsyncedPwm, uint16_t idlePulse)
 210 {
 211     uint32_t timerMhzCounter;
 212
 213     switch (fastPwmProtocolType) {
 214         default:
 215         case (PWM_TYPE_ONESHOT125):
 216             timerMhzCounter = ONESHOT125_TIMER_MHZ;
 217             break;
 218         case (PWM_TYPE_ONESHOT42):
 219             timerMhzCounter = ONESHOT42_TIMER_MHZ;
 220             break;
 221         case (PWM_TYPE_MULTISHOT):
 222             timerMhzCounter = MULTISHOT_TIMER_MHZ;
 223     }
 224
 225     if (useUnsyncedPwm) {
 226         uint32_t hz = timerMhzCounter * 1000000;
 227         motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse);
 228     } else {
 229         motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0);
 230     }
 231
 232     motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot : pwmWriteStandard;
 233 }
 234
 235 #ifdef USE_SERVOS
 236 void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse)
 237 {
 238     servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse);
 239 }
 240
 241 void pwmWriteServo(uint8_t index, uint16_t value)
 242 {
 243     if (servos[index] && index < MAX_SERVOS)
 244         *servos[index]->ccr = value;
 245 }
 246 #endif