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