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 <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24 #include "drivers/time.h"
  25
  26 #include "drivers/io.h"
  27 #include "pwm_output.h"
  28 #include "timer.h"
  29 #include "drivers/pwm_output.h"
  30
  31 static pwmWriteFunc *pwmWrite;
  32 static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
  33 static pwmCompleteWriteFunc *pwmCompleteWrite = NULL;
  34
  35 #ifdef USE_DSHOT
  36 loadDmaBufferFunc *loadDmaBuffer;
  37 #endif
  38
  39 #ifdef USE_SERVOS
  40 static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
  41 #endif
  42
  43 #ifdef BEEPER
  44 static pwmOutputPort_t beeperPwm;
  45 static uint16_t freqBeep = 0;
  46 #endif
  47
  48 bool pwmMotorsEnabled = false;
  49 bool isDshot = false;
  50
  51 static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
  52 {
  53 #if defined(USE_HAL_DRIVER)
  54     TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
  55     if(Handle == NULL) return;
  56
  57     TIM_OC_InitTypeDef TIM_OCInitStructure;
  58
  59     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
  60
  61     if (output & TIMER_OUTPUT_N_CHANNEL) {
  62         TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
  63         TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_HIGH: TIM_OCPOLARITY_LOW;
  64         TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
  65         TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_HIGH : TIM_OCNPOLARITY_LOW;
  66     } else {
  67         TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
  68         TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
  69         TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET;
  70         TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH;
  71     }
  72
  73     TIM_OCInitStructure.Pulse = value;
  74     TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
  75
  76     HAL_TIM_PWM_ConfigChannel(Handle, &TIM_OCInitStructure, channel);
  77 #else
  78     TIM_OCInitTypeDef TIM_OCInitStructure;
  79
  80     TIM_OCStructInit(&TIM_OCInitStructure);
  81     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  82
  83     if (output & TIMER_OUTPUT_N_CHANNEL) {
  84         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
  85         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
  86         TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_High : TIM_OCNPolarity_Low;
  87     } else {
  88         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  89         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
  90         TIM_OCInitStructure.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
  91     }
  92     TIM_OCInitStructure.TIM_Pulse = value;
  93
  94     timerOCInit(tim, channel, &TIM_OCInitStructure);
  95     timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable);
  96 #endif
  97 }
  98
  99 static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
 100 {
 101 #if defined(USE_HAL_DRIVER)
 102     TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
 103     if(Handle == NULL) return;
 104 #endif
 105
 106     configTimeBase(timerHardware->tim, period, hz);
 107     pwmOCConfig(timerHardware->tim,
 108         timerHardware->channel,
 109         value,
 110         inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
 111         );
 112
 113 #if defined(USE_HAL_DRIVER)
 114     if(timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
 115         HAL_TIMEx_PWMN_Start(Handle, timerHardware->channel);
 116     else
 117         HAL_TIM_PWM_Start(Handle, timerHardware->channel);
 118     HAL_TIM_Base_Start(Handle);
 119 #else
 120     TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
 121     TIM_Cmd(timerHardware->tim, ENABLE);
 122 #endif
 123
 124     port->ccr = timerChCCR(timerHardware);
 125
 126     port->tim = timerHardware->tim;
 127
 128     *port->ccr = 0;
 129 }
 130
 131 static void pwmWriteUnused(uint8_t index, float value)
 132 {
 133     UNUSED(index);
 134     UNUSED(value);
 135 }
 136
 137 static void pwmWriteStandard(uint8_t index, float value)
 138 {
 139     /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
 140     *motors[index].ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
 141 }
 142
 143 #ifdef USE_DSHOT
 144 static void pwmWriteDshot(uint8_t index, float value)
 145 {
 146     pwmWriteDshotInt(index, lrintf(value));
 147 }
 148
 149 static uint8_t loadDmaBufferDshot(motorDmaOutput_t *const motor, uint16_t packet)
 150 {
 151     for (int i = 0; i < 16; i++) {
 152         motor->dmaBuffer[i] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0;  // MSB first
 153         packet <<= 1;
 154         }
 155
 156     return DSHOT_DMA_BUFFER_SIZE;
 157 }
 158
 159 static uint8_t loadDmaBufferProshot(motorDmaOutput_t *const motor, uint16_t packet)
 160 {
 161     for (int i = 0; i < 4; i++) {
 162         motor->dmaBuffer[i] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH;  // Most significant nibble first
 163         packet <<= 4;   // Shift 4 bits
 164     }
 165
 166     return PROSHOT_DMA_BUFFER_SIZE;
 167 }
 168 #endif
 169
 170 void pwmWriteMotor(uint8_t index, float value)
 171 {
 172     if (pwmMotorsEnabled) {
 173         pwmWrite(index, value);
 174     }
 175 }
 176
 177 void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
 178 {
 179     for (int index = 0; index < motorCount; index++) {
 180         // Set the compare register to 0, which stops the output pulsing if the timer overflows
 181         if (motors[index].ccr) {
 182             *motors[index].ccr = 0;
 183         }
 184     }
 185 }
 186
 187 void pwmDisableMotors(void)
 188 {
 189     pwmShutdownPulsesForAllMotors(MAX_SUPPORTED_MOTORS);
 190     pwmMotorsEnabled = false;
 191 }
 192
 193 void pwmEnableMotors(void)
 194 {
 195     /* check motors can be enabled */
 196     pwmMotorsEnabled = (pwmWrite != &pwmWriteUnused);
 197 }
 198
 199 bool pwmAreMotorsEnabled(void)
 200 {
 201     return pwmMotorsEnabled;
 202 }
 203
 204 static void pwmCompleteWriteUnused(uint8_t motorCount)
 205 {
 206     UNUSED(motorCount);
 207 }
 208
 209 static void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
 210 {
 211     for (int index = 0; index < motorCount; index++) {
 212         if (motors[index].forceOverflow) {
 213             timerForceOverflow(motors[index].tim);
 214         }
 215         // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
 216         // This compare register will be set to the output value on the next main loop.
 217         *motors[index].ccr = 0;
 218     }
 219 }
 220
 221 void pwmCompleteMotorUpdate(uint8_t motorCount)
 222 {
 223     pwmCompleteWrite(motorCount);
 224 }
 225
 226 void motorDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
 227 {
 228     memset(motors, 0, sizeof(motors));
 229
 230     bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
 231
 232     float sMin = 0;
 233     float sLen = 0;
 234     switch (motorConfig->motorPwmProtocol) {
 235     default:
 236     case PWM_TYPE_ONESHOT125:
 237         sMin = 125e-6f;
 238         sLen = 125e-6f;
 239         break;
 240     case PWM_TYPE_ONESHOT42:
 241         sMin = 42e-6f;
 242         sLen = 42e-6f;
 243         break;
 244     case PWM_TYPE_MULTISHOT:
 245         sMin = 5e-6f;
 246         sLen = 20e-6f;
 247         break;
 248     case PWM_TYPE_BRUSHED:
 249         sMin = 0;
 250         useUnsyncedPwm = true;
 251         idlePulse = 0;
 252         break;
 253     case PWM_TYPE_STANDARD:
 254         sMin = 1e-3f;
 255         sLen = 1e-3f;
 256         useUnsyncedPwm = true;
 257         idlePulse = 0;
 258         break;
 259 #ifdef USE_DSHOT
 260     case PWM_TYPE_PROSHOT1000:
 261         pwmWrite = &pwmWriteDshot;
 262         loadDmaBuffer = &loadDmaBufferProshot;
 263         pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
 264         isDshot = true;
 265         break;
 266     case PWM_TYPE_DSHOT1200:
 267     case PWM_TYPE_DSHOT600:
 268     case PWM_TYPE_DSHOT300:
 269     case PWM_TYPE_DSHOT150:
 270         pwmWrite = &pwmWriteDshot;
 271         loadDmaBuffer = &loadDmaBufferDshot;
 272         pwmCompleteWrite = &pwmCompleteDshotMotorUpdate;
 273         isDshot = true;
 274         break;
 275 #endif
 276     }
 277
 278     if (!isDshot) {
 279         pwmWrite = &pwmWriteStandard;
 280         pwmCompleteWrite = useUnsyncedPwm ? &pwmCompleteWriteUnused : &pwmCompleteOneshotMotorUpdate;
 281     }
 282
 283     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 284         const ioTag_t tag = motorConfig->ioTags[motorIndex];
 285         const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
 286
 287         if (timerHardware == NULL) {
 288             /* not enough motors initialised for the mixer or a break in the motors */
 289             pwmWrite = &pwmWriteUnused;
 290             pwmCompleteWrite = &pwmCompleteWriteUnused;
 291             /* TODO: block arming and add reason system cannot arm */
 292             return;
 293         }
 294
 295         motors[motorIndex].io = IOGetByTag(tag);
 296
 297 #ifdef USE_DSHOT
 298         if (isDshot) {
 299             pwmDshotMotorHardwareConfig(timerHardware,
 300                 motorIndex,
 301                 motorConfig->motorPwmProtocol,
 302                 motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output);
 303             motors[motorIndex].enabled = true;
 304             continue;
 305         }
 306 #endif
 307
 308         IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
 309 #if defined(USE_HAL_DRIVER)
 310         IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
 311 #else
 312         IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
 313 #endif
 314
 315         /* standard PWM outputs */
 316         const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / (sMin + sLen * 2));
 317
 318         const uint32_t clock = timerClock(timerHardware->tim);
 319         /* used to find the desired timer frequency for max resolution */
 320         const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
 321         const uint32_t hz = clock / prescaler;
 322         const unsigned period = hz / pwmRateHz;
 323
 324         /*
 325             if brushed then it is the entire length of the period.
 326             TODO: this can be moved back to periodMin and periodLen
 327             once mixer outputs a 0..1 float value.
 328         */
 329         motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
 330         motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
 331
 332         pwmOutConfig(&motors[motorIndex], timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
 333
 334         bool timerAlreadyUsed = false;
 335         for (int i = 0; i < motorIndex; i++) {
 336             if (motors[i].tim == motors[motorIndex].tim) {
 337                 timerAlreadyUsed = true;
 338                 break;
 339             }
 340         }
 341         motors[motorIndex].forceOverflow = !timerAlreadyUsed;
 342         motors[motorIndex].enabled = true;
 343     }
 344
 345     pwmMotorsEnabled = true;
 346 }
 347
 348 pwmOutputPort_t *pwmGetMotors(void)
 349 {
 350     return motors;
 351 }
 352
 353 bool isMotorProtocolDshot(void)
 354 {
 355     return isDshot;
 356 }
 357
 358 #ifdef USE_DSHOT
 359 uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
 360 {
 361     switch (pwmProtocolType) {
 362     case(PWM_TYPE_PROSHOT1000):
 363         return MOTOR_PROSHOT1000_HZ;
 364     case(PWM_TYPE_DSHOT1200):
 365         return MOTOR_DSHOT1200_HZ;
 366     case(PWM_TYPE_DSHOT600):
 367         return MOTOR_DSHOT600_HZ;
 368     case(PWM_TYPE_DSHOT300):
 369         return MOTOR_DSHOT300_HZ;
 370     default:
 371     case(PWM_TYPE_DSHOT150):
 372         return MOTOR_DSHOT150_HZ;
 373     }
 374 }
 375
 376 void pwmWriteDshotCommand(uint8_t index, uint8_t command)
 377 {
 378     if (isDshot && (command <= DSHOT_MAX_COMMAND)) {
 379         motorDmaOutput_t *const motor = getMotorDmaOutput(index);
 380
 381         unsigned repeats;
 382         switch (command) {
 383         case DSHOT_CMD_SPIN_DIRECTION_1:
 384         case DSHOT_CMD_SPIN_DIRECTION_2:
 385         case DSHOT_CMD_3D_MODE_OFF:
 386         case DSHOT_CMD_3D_MODE_ON:
 387         case DSHOT_CMD_SAVE_SETTINGS:
 388         case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
 389         case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
 390             repeats = 10;
 391             break;
 392         default:
 393             repeats = 1;
 394             break;
 395         }
 396
 397         for (; repeats; repeats--) {
 398             motor->requestTelemetry = true;
 399             pwmWriteDshotInt(index, command);
 400             pwmCompleteMotorUpdate(0);
 401
 402             delay(1);
 403         }
 404     }
 405 }
 406
 407 uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value)
 408 {
 409     uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0);
 410     motor->requestTelemetry = false;    // reset telemetry request to make sure it's triggered only once in a row
 411
 412     // compute checksum
 413     int csum = 0;
 414     int csum_data = packet;
 415     for (int i = 0; i < 3; i++) {
 416         csum ^=  csum_data;   // xor data by nibbles
 417         csum_data >>= 4;
 418     }
 419     csum &= 0xf;
 420     // append checksum
 421     packet = (packet << 4) | csum;
 422
 423     return packet;
 424 }
 425 #endif
 426
 427 #ifdef USE_SERVOS
 428 void pwmWriteServo(uint8_t index, float value)
 429 {
 430     if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
 431         *servos[index].ccr = lrintf(value);
 432     }
 433 }
 434
 435 void servoDevInit(const servoDevConfig_t *servoConfig)
 436 {
 437     for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
 438         const ioTag_t tag = servoConfig->ioTags[servoIndex];
 439
 440         if (!tag) {
 441             break;
 442         }
 443
 444         servos[servoIndex].io = IOGetByTag(tag);
 445
 446         IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
 447
 448         const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
 449 #if defined(USE_HAL_DRIVER)
 450         IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
 451 #else
 452         IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
 453 #endif
 454
 455         if (timer == NULL) {
 456             /* flag failure and disable ability to arm */
 457             break;
 458         }
 459         pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
 460         servos[servoIndex].enabled = true;
 461     }
 462 }
 463
 464 #endif
 465
 466 #ifdef BEEPER
 467 void pwmWriteBeeper(bool onoffBeep)
 468 {
 469         if(!beeperPwm.io)
 470             return;
 471         if(onoffBeep == true) {
 472             *beeperPwm.ccr = (PWM_TIMER_1MHZ / freqBeep) / 2;
 473             beeperPwm.enabled = true;
 474         } else {
 475             *beeperPwm.ccr = 0;
 476             beeperPwm.enabled = false;
 477         }
 478 }
 479
 480 void pwmToggleBeeper(void)
 481 {
 482         pwmWriteBeeper(!beeperPwm.enabled);
 483 }
 484
 485 void beeperPwmInit(IO_t io, uint16_t frequency)
 486 {
 487         const ioTag_t tag=IO_TAG(BEEPER);
 488         beeperPwm.io = io;
 489         const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_BEEPER);
 490         if (beeperPwm.io && timer) {
 491             IOInit(beeperPwm.io, OWNER_BEEPER, RESOURCE_INDEX(0));
 492 #if defined(USE_HAL_DRIVER)
 493             IOConfigGPIOAF(beeperPwm.io, IOCFG_AF_PP, timer->alternateFunction);
 494 #else
 495             IOConfigGPIO(beeperPwm.io, IOCFG_AF_PP);
 496 #endif
 497             freqBeep = frequency;
 498             pwmOutConfig(&beeperPwm, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / freqBeep, (PWM_TIMER_1MHZ / freqBeep) / 2, 0);
 499         }
 500         *beeperPwm.ccr = 0;
 501         beeperPwm.enabled = false;
 502 }
 503 #endif