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 #include <math.h>
  21
  22 #include "platform.h"
  23
  24 #include "io.h"
  25 #include "timer.h"
  26 #include "pwm_output.h"
  27
  28 #define MULTISHOT_5US_PW    (MULTISHOT_TIMER_MHZ * 5)
  29 #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
  30
  31 static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
  32 static pwmCompleteWriteFuncPtr pwmCompleteWritePtr = NULL;
  33
  34 #ifdef USE_SERVOS
  35 static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
  36 #endif
  37
  38 bool pwmMotorsEnabled = true;
  39
  40 static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
  41 {
  42     TIM_OCInitTypeDef TIM_OCInitStructure;
  43
  44     TIM_OCStructInit(&TIM_OCInitStructure);
  45     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  46
  47     if (output & TIMER_OUTPUT_N_CHANNEL) {
  48         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
  49         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
  50         TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
  51     } else {
  52         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  53         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
  54         TIM_OCInitStructure.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
  55     }
  56     TIM_OCInitStructure.TIM_Pulse = value;
  57
  58     timerOCInit(tim, channel, &TIM_OCInitStructure);
  59     timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable);
  60 }
  61
  62 static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
  63 {
  64     configTimeBase(timerHardware->tim, period, mhz);
  65     pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
  66
  67     if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
  68         TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
  69     }
  70     TIM_Cmd(timerHardware->tim, ENABLE);
  71
  72     port->ccr = timerChCCR(timerHardware);
  73     port->period = period;
  74     port->tim = timerHardware->tim;
  75
  76     *port->ccr = 0;
  77 }
  78
  79 static void pwmWriteBrushed(uint8_t index, uint16_t value)
  80 {
  81     *motors[index].ccr = (value - 1000) * motors[index].period / 1000;
  82 }
  83
  84 static void pwmWriteStandard(uint8_t index, uint16_t value)
  85 {
  86     *motors[index].ccr = value;
  87 }
  88
  89 static void pwmWriteOneShot125(uint8_t index, uint16_t value)
  90 {
  91     *motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f));
  92 }
  93
  94 static void pwmWriteOneShot42(uint8_t index, uint16_t value)
  95 {
  96     *motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
  97 }
  98
  99 static void pwmWriteMultiShot(uint8_t index, uint16_t value)
 100 {
 101     *motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
 102 }
 103
 104 void pwmWriteMotor(uint8_t index, uint16_t value)
 105 {
 106     if (index < MAX_SUPPORTED_MOTORS && pwmMotorsEnabled && motors[index].pwmWritePtr) {
 107         motors[index].pwmWritePtr(index, value);
 108     }
 109 }
 110
 111 void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
 112 {
 113     for (int index = 0; index < motorCount; index++) {
 114         // Set the compare register to 0, which stops the output pulsing if the timer overflows
 115         if (motors[index].ccr) {
 116             *motors[index].ccr = 0;
 117         }
 118     }
 119 }
 120
 121 void pwmDisableMotors(void)
 122 {
 123     pwmShutdownPulsesForAllMotors(MAX_SUPPORTED_MOTORS);
 124     pwmMotorsEnabled = false;
 125 }
 126
 127 void pwmEnableMotors(void)
 128 {
 129     pwmMotorsEnabled = true;
 130 }
 131
 132 static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
 133 {
 134     for (int index = 0; index < motorCount; index++) {
 135         bool overflowed = false;
 136         // If we have not already overflowed this timer
 137         for (int j = 0; j < index; j++) {
 138             if (motors[j].tim == motors[index].tim) {
 139                 overflowed = true;
 140                 break;
 141             }
 142         }
 143         if (!overflowed) {
 144             timerForceOverflow(motors[index].tim);
 145         }
 146         // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
 147         // This compare register will be set to the output value on the next main loop.
 148         *motors[index].ccr = 0;
 149     }
 150 }
 151
 152 void pwmCompleteMotorUpdate(uint8_t motorCount)
 153 {
 154     if (pwmCompleteWritePtr) {
 155         pwmCompleteWritePtr(motorCount);
 156     }
 157 }
 158
 159 void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
 160 {
 161     uint32_t timerMhzCounter = 0;
 162     pwmWriteFuncPtr pwmWritePtr;
 163     bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
 164     bool isDigital = false;
 165
 166     switch (motorConfig->motorPwmProtocol) {
 167     default:
 168     case PWM_TYPE_ONESHOT125:
 169         timerMhzCounter = ONESHOT125_TIMER_MHZ;
 170         pwmWritePtr = pwmWriteOneShot125;
 171         break;
 172     case PWM_TYPE_ONESHOT42:
 173         timerMhzCounter = ONESHOT42_TIMER_MHZ;
 174         pwmWritePtr = pwmWriteOneShot42;
 175         break;
 176     case PWM_TYPE_MULTISHOT:
 177         timerMhzCounter = MULTISHOT_TIMER_MHZ;
 178         pwmWritePtr = pwmWriteMultiShot;
 179         break;
 180     case PWM_TYPE_BRUSHED:
 181         timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
 182         pwmWritePtr = pwmWriteBrushed;
 183         useUnsyncedPwm = true;
 184         idlePulse = 0;
 185         break;
 186     case PWM_TYPE_STANDARD:
 187         timerMhzCounter = PWM_TIMER_MHZ;
 188         pwmWritePtr = pwmWriteStandard;
 189         useUnsyncedPwm = true;
 190         idlePulse = 0;
 191         break;
 192 #ifdef USE_DSHOT
 193     case PWM_TYPE_DSHOT600:
 194     case PWM_TYPE_DSHOT300:
 195     case PWM_TYPE_DSHOT150:
 196         pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
 197         isDigital = true;
 198         break;
 199 #endif
 200     }
 201
 202     if (!useUnsyncedPwm && !isDigital) {
 203         pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate;
 204     }
 205
 206     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 207         const ioTag_t tag = motorConfig->ioTags[motorIndex];
 208
 209         if (!tag) {
 210             break;
 211         }
 212
 213         const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
 214
 215         if (timerHardware == NULL) {
 216             /* flag failure and disable ability to arm */
 217             break;
 218         }
 219
 220         motors[motorIndex].io = IOGetByTag(tag);
 221
 222 #ifdef USE_DSHOT
 223         if (isDigital) {
 224             pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
 225             motors[motorIndex].pwmWritePtr = pwmWriteDigital;
 226             motors[motorIndex].enabled = true;
 227             continue;
 228         }
 229 #endif
 230
 231         IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
 232         IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
 233
 234         motors[motorIndex].pwmWritePtr = pwmWritePtr;
 235         if (useUnsyncedPwm) {
 236             const uint32_t hz = timerMhzCounter * 1000000;
 237             pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse);
 238         } else {
 239             pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0);
 240         }
 241         motors[motorIndex].enabled = true;
 242     }
 243 }
 244
 245 bool pwmIsSynced(void)
 246 {
 247     return pwmCompleteWritePtr != NULL;
 248 }
 249
 250 pwmOutputPort_t *pwmGetMotors(void)
 251 {
 252     return motors;
 253 }
 254
 255 #ifdef USE_SERVOS
 256 void pwmWriteServo(uint8_t index, uint16_t value)
 257 {
 258     if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
 259         *servos[index].ccr = value;
 260     }
 261 }
 262
 263 void servoInit(const servoConfig_t *servoConfig)
 264 {
 265     for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
 266         const ioTag_t tag = servoConfig->ioTags[servoIndex];
 267
 268         if (!tag) {
 269             break;
 270         }
 271
 272         servos[servoIndex].io = IOGetByTag(tag);
 273
 274         IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
 275         IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
 276
 277         const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
 278
 279         if (timer == NULL) {
 280             /* flag failure and disable ability to arm */
 281             break;
 282         }
 283
 284         pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
 285         servos[servoIndex].enabled = true;
 286     }
 287 }
 288
 289 #endif