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 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #if !defined(SITL_BUILD)
  26
  27 #include "build/debug.h"
  28
  29 #include "common/log.h"
  30 #include "common/maths.h"
  31 #include "common/circular_queue.h"
  32
  33 #include "drivers/io.h"
  34 #include "drivers/timer.h"
  35 #include "drivers/pwm_mapping.h"
  36 #include "drivers/pwm_output.h"
  37 #include "io/servo_sbus.h"
  38 #include "sensors/esc_sensor.h"
  39
  40 #include "config/feature.h"
  41
  42 #include "fc/config.h"
  43 #include "fc/runtime_config.h"
  44
  45 #include "drivers/timer_impl.h"
  46 #include "drivers/timer.h"
  47
  48 #define MULTISHOT_5US_PW    (MULTISHOT_TIMER_HZ * 5 / 1000000.0f)
  49 #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_HZ * 20 / 1000000.0f / 1000.0f)
  50
  51 #ifdef USE_DSHOT
  52 #define MOTOR_DSHOT600_HZ     12000000
  53 #define MOTOR_DSHOT300_HZ     6000000
  54 #define MOTOR_DSHOT150_HZ     3000000
  55
  56
  57 #define DSHOT_MOTOR_BIT_0       7
  58 #define DSHOT_MOTOR_BIT_1       14
  59 #define DSHOT_MOTOR_BITLENGTH   20
  60
  61 #define DSHOT_DMA_BUFFER_SIZE   18 /* resolution + frame reset (2us) */
  62 #define MAX_DMA_TIMERS          8
  63
  64 #define DSHOT_COMMAND_DELAY_US 1000
  65 #define DSHOT_COMMAND_INTERVAL_US 10000
  66 #define DSHOT_COMMAND_QUEUE_LENGTH 8
  67 #define DHSOT_COMMAND_QUEUE_SIZE   DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e)
  68 #endif
  69
  70 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
  71
  72 #ifdef USE_DSHOT_DMAR
  73     timerDMASafeType_t dmaBurstBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
  74 #endif
  75
  76 typedef struct {
  77     TCH_t * tch;
  78     bool configured;
  79     uint16_t value;
  80
  81     // PWM parameters
  82     volatile timCCR_t *ccr;         // Shortcut for timer CCR register
  83     float pulseOffset;
  84     float pulseScale;
  85
  86 #ifdef USE_DSHOT
  87     // DSHOT parameters
  88     timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE];
  89 #ifdef USE_DSHOT_DMAR
  90     timerDMASafeType_t *dmaBurstBuffer;
  91 #endif
  92 #endif
  93 } pwmOutputPort_t;
  94
  95 typedef struct {
  96     pwmOutputPort_t *   pwmPort;        // May be NULL if motor doesn't use the PWM port
  97     uint16_t            value;          // Used to keep track of last motor value
  98     bool                requestTelemetry;
  99 } pwmOutputMotor_t;
 100
 101 static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUTS];
 102
 103 static pwmOutputMotor_t        motors[MAX_MOTORS];
 104 static motorPwmProtocolTypes_e initMotorProtocol;
 105 static pwmWriteFuncPtr         motorWritePtr = NULL;    // Function to write value to motors
 106
 107 static pwmOutputPort_t *       servos[MAX_SERVOS];
 108 static pwmWriteFuncPtr         servoWritePtr = NULL;    // Function to write value to motors
 109
 110 static pwmOutputPort_t  beeperPwmPort;
 111 static pwmOutputPort_t *beeperPwm;
 112 static uint16_t beeperFrequency = 0;
 113
 114 static uint8_t allocatedOutputPortCount = 0;
 115
 116 static bool pwmMotorsEnabled = true;
 117
 118 #ifdef USE_DSHOT
 119 static timeUs_t digitalMotorUpdateIntervalUs = 0;
 120 static timeUs_t digitalMotorLastUpdateUs;
 121 static timeUs_t lastCommandSent = 0;
 122 static timeUs_t commandPostDelay = 0;
 123
 124 static circularBuffer_t commandsCircularBuffer;
 125 static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE];
 126 static currentExecutingCommand_t currentExecutingCommand;
 127 #endif
 128
 129 static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value)
 130 {
 131     p->tch = tch;
 132
 133     timerConfigBase(p->tch, period, hz);
 134     timerPWMConfigChannel(p->tch, value);
 135     timerPWMStart(p->tch);
 136
 137     timerEnable(p->tch);
 138
 139     p->ccr = timerCCR(p->tch);
 140     *p->ccr = 0;
 141 }
 142
 143 static pwmOutputPort_t *pwmOutAllocatePort(void)
 144 {
 145     if (allocatedOutputPortCount >= MAX_PWM_OUTPUTS) {
 146         LOG_ERROR(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS");
 147         return NULL;
 148     }
 149
 150     pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
 151
 152     p->tch = NULL;
 153     p->configured = false;
 154
 155     return p;
 156 }
 157
 158 static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timHw, resourceOwner_e owner, uint32_t hz, uint16_t period, uint16_t value, bool enableOutput)
 159 {
 160     // Attempt to allocate TCH
 161     TCH_t * tch = timerGetTCH(timHw);
 162     if (tch == NULL) {
 163         return NULL;
 164     }
 165
 166     // Allocate motor output port
 167     pwmOutputPort_t *p = pwmOutAllocatePort();
 168     if (p == NULL) {
 169         return NULL;
 170     }
 171
 172     const IO_t io = IOGetByTag(timHw->tag);
 173     IOInit(io, owner, RESOURCE_OUTPUT, allocatedOutputPortCount);
 174
 175     pwmOutConfigTimer(p, tch, hz, period, value);
 176
 177     if (enableOutput) {
 178         IOConfigGPIOAF(io, IOCFG_AF_PP, timHw->alternateFunction);
 179     }
 180     else {
 181         // If PWM outputs are disabled - configure as GPIO and drive low
 182         IOConfigGPIO(io, IOCFG_OUT_OD);
 183         IOLo(io);
 184     }
 185
 186     return p;
 187 }
 188
 189 static void pwmWriteNull(uint8_t index, uint16_t value)
 190 {
 191     (void)index;
 192     (void)value;
 193 }
 194
 195 static void pwmWriteStandard(uint8_t index, uint16_t value)
 196 {
 197     if (motors[index].pwmPort) {
 198         *(motors[index].pwmPort->ccr) = lrintf((value * motors[index].pwmPort->pulseScale) + motors[index].pwmPort->pulseOffset);
 199     }
 200 }
 201
 202 void pwmWriteMotor(uint8_t index, uint16_t value)
 203 {
 204     if (motorWritePtr && index < MAX_MOTORS && pwmMotorsEnabled) {
 205         motorWritePtr(index, value);
 206     }
 207 }
 208
 209 void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
 210 {
 211     for (int index = 0; index < motorCount; index++) {
 212         // Set the compare register to 0, which stops the output pulsing if the timer overflows
 213         if (motors[index].pwmPort) {
 214             *(motors[index].pwmPort->ccr) = 0;
 215         }
 216     }
 217 }
 218
 219 void pwmDisableMotors(void)
 220 {
 221     pwmMotorsEnabled = false;
 222 }
 223
 224 void pwmEnableMotors(void)
 225 {
 226     pwmMotorsEnabled = true;
 227 }
 228
 229 bool isMotorBrushed(uint16_t motorPwmRateHz)
 230 {
 231     return (motorPwmRateHz > 500);
 232 }
 233
 234 static pwmOutputPort_t * motorConfigPwm(const timerHardware_t *timerHardware, float sMin, float sLen, uint32_t motorPwmRateHz, bool enableOutput)
 235 {
 236     const uint32_t baseClockHz = timerGetBaseClockHW(timerHardware);
 237     const uint32_t prescaler = ((baseClockHz / motorPwmRateHz) + 0xffff) / 0x10000; /* rounding up */
 238     const uint32_t timerHz = baseClockHz / prescaler;
 239     const uint32_t period = timerHz / motorPwmRateHz;
 240
 241     pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_MOTOR, timerHz, period, 0, enableOutput);
 242
 243     if (port) {
 244         port->pulseScale = ((sLen == 0) ? period : (sLen * timerHz)) / 1000.0f;
 245         port->pulseOffset = (sMin * timerHz) - (port->pulseScale * 1000);
 246         port->configured = true;
 247     }
 248
 249     return port;
 250 }
 251
 252 #ifdef USE_DSHOT
 253 uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
 254 {
 255     switch (pwmProtocolType) {
 256         case(PWM_TYPE_DSHOT600):
 257             return MOTOR_DSHOT600_HZ;
 258         case(PWM_TYPE_DSHOT300):
 259             return MOTOR_DSHOT300_HZ;
 260         default:
 261         case(PWM_TYPE_DSHOT150):
 262             return MOTOR_DSHOT150_HZ;
 263     }
 264 }
 265
 266 #ifdef USE_DSHOT_DMAR
 267 burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS];
 268 uint8_t burstDmaTimersCount = 0;
 269
 270 static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer)
 271 {
 272     for (int i = 0; i < burstDmaTimersCount; i++) {
 273         if (burstDmaTimers[i].timer == timer) {
 274             return i;
 275         }
 276     }
 277     burstDmaTimers[burstDmaTimersCount++].timer = timer;
 278     return burstDmaTimersCount - 1;
 279 }
 280 #endif
 281
 282 static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput)
 283 {
 284     // Try allocating new port
 285     pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_MOTOR, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput);
 286
 287     if (!port) {
 288         return NULL;
 289     }
 290
 291     // Configure timer DMA
 292 #ifdef USE_DSHOT_DMAR
 293     //uint8_t timerIndex = lookupTimerIndex(timerHardware->tim);
 294     uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim);
 295     if (burstDmaTimerIndex >= MAX_DMA_TIMERS) {
 296         return NULL;
 297     }
 298
 299     port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0];
 300     burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
 301     burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer;
 302
 303     if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
 304         port->configured = true;
 305     }
 306 #else
 307     if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) {
 308         // Only mark as DSHOT channel if DMA was set successfully
 309         ZERO_FARRAY(port->dmaBuffer);
 310         port->configured = true;
 311     }
 312 #endif
 313
 314     return port;
 315 }
 316
 317 #ifdef USE_DSHOT_DMAR
 318 static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, uint16_t packet)
 319 {
 320     int i;
 321     for (i = 0; i < 16; i++) {
 322         dmaBuffer[i * stride] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0;  // MSB first
 323         packet <<= 1;
 324     }
 325     dmaBuffer[i++ * stride] = 0;
 326     dmaBuffer[i++ * stride] = 0;
 327 }
 328 #else
 329 static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet)
 330 {
 331     for (int i = 0; i < 16; i++) {
 332         dmaBuffer[i] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0;  // MSB first
 333         packet <<= 1;
 334     }
 335 }
 336 #endif
 337
 338 static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry)
 339 {
 340     uint16_t packet = (value << 1) | (requestTelemetry ? 1 : 0);
 341
 342     // compute checksum
 343     int csum = 0;
 344     int csum_data = packet;
 345     for (int i = 0; i < 3; i++) {
 346         csum ^=  csum_data;   // xor data by nibbles
 347         csum_data >>= 4;
 348     }
 349     csum &= 0xf;
 350
 351     // append checksum
 352     packet = (packet << 4) | csum;
 353
 354     return packet;
 355 }
 356 #endif
 357
 358 #if defined(USE_DSHOT)
 359 static void motorConfigDigitalUpdateInterval(uint16_t motorPwmRateHz)
 360 {
 361     digitalMotorUpdateIntervalUs = 1000000 / motorPwmRateHz;
 362     digitalMotorLastUpdateUs = 0;
 363 }
 364
 365 static void pwmWriteDigital(uint8_t index, uint16_t value)
 366 {
 367     // Just keep track of motor value, actual update happens in pwmCompleteMotorUpdate()
 368     // DSHOT and some other digital protocols use 11-bit throttle range [0;2047]
 369     motors[index].value = constrain(value, 0, 2047);
 370 }
 371
 372 bool isMotorProtocolDshot(void)
 373 {
 374     // We look at cached `initMotorProtocol` to make sure we are consistent with the initialized config
 375     // motorConfig()->motorPwmProtocol may change at run time which will cause uninitialized structures to be used
 376     return getMotorProtocolProperties(initMotorProtocol)->isDSHOT;
 377 }
 378
 379 bool isMotorProtocolDigital(void)
 380 {
 381     return isMotorProtocolDshot();
 382 }
 383
 384 void pwmRequestMotorTelemetry(int motorIndex)
 385 {
 386     if (!isMotorProtocolDigital()) {
 387         return;
 388     }
 389
 390     const int motorCount = getMotorCount();
 391     for (int index = 0; index < motorCount; index++) {
 392         if (motors[index].pwmPort && motors[index].pwmPort->configured && index == motorIndex) {
 393             motors[index].requestTelemetry = true;
 394         }
 395     }
 396 }
 397
 398 #ifdef USE_DSHOT
 399 void sendDShotCommand(dshotCommands_e cmd) {
 400     circularBufferPushElement(&commandsCircularBuffer, (uint8_t *) &cmd);
 401 }
 402
 403 void initDShotCommands(void) {
 404     circularBufferInit(&commandsCircularBuffer, commandsBuff,DHSOT_COMMAND_QUEUE_SIZE, sizeof(dshotCommands_e));
 405
 406     currentExecutingCommand.remainingRepeats = 0;
 407 }
 408
 409 static int getDShotCommandRepeats(dshotCommands_e cmd) {
 410     int repeats = 1;
 411
 412     switch (cmd) {
 413         case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
 414         case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
 415             repeats = 10;
 416             break;
 417         default:
 418             break;
 419     }
 420
 421     return repeats;
 422 }
 423
 424 static bool executeDShotCommands(void){
 425
 426     timeUs_t tNow = micros();
 427
 428     if(currentExecutingCommand.remainingRepeats == 0) {
 429        const int isTherePendingCommands = !circularBufferIsEmpty(&commandsCircularBuffer);
 430         if (isTherePendingCommands && (tNow - lastCommandSent > DSHOT_COMMAND_INTERVAL_US)){
 431             //Load the command
 432             dshotCommands_e cmd;
 433             circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &cmd);
 434             currentExecutingCommand.cmd = cmd;
 435             currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd);
 436             commandPostDelay = DSHOT_COMMAND_INTERVAL_US;
 437         } else {
 438             if (commandPostDelay) {
 439                 if (tNow - lastCommandSent < commandPostDelay) {
 440                     return false;
 441                 }
 442                 commandPostDelay = 0;
 443             }
 444
 445             return true;
 446         }
 447     }
 448     for (uint8_t i = 0; i < getMotorCount(); i++) {
 449          motors[i].requestTelemetry = true;
 450          motors[i].value = currentExecutingCommand.cmd;
 451     }
 452     if (tNow - lastCommandSent >= DSHOT_COMMAND_DELAY_US) {
 453         currentExecutingCommand.remainingRepeats--;
 454         lastCommandSent = tNow;
 455         return true;
 456     } else {
 457         return false;
 458     }
 459 }
 460 #endif
 461
 462 void pwmCompleteMotorUpdate(void) {
 463     // This only makes sense for digital motor protocols
 464     if (!isMotorProtocolDigital()) {
 465         return;
 466     }
 467
 468     int motorCount = getMotorCount();
 469     timeUs_t currentTimeUs = micros();
 470
 471     // Enforce motor update rate
 472     if ((digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) {
 473         return;
 474     }
 475
 476     digitalMotorLastUpdateUs = currentTimeUs;
 477
 478 #ifdef USE_DSHOT
 479     if (isMotorProtocolDshot()) {
 480
 481         if (!executeDShotCommands()) {
 482             return;
 483         }
 484
 485 #ifdef USE_DSHOT_DMAR
 486         for (int index = 0; index < motorCount; index++) {
 487             if (motors[index].pwmPort && motors[index].pwmPort->configured) {
 488                 uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry);
 489                 loadDmaBufferDshotStride(&motors[index].pwmPort->dmaBurstBuffer[motors[index].pwmPort->tch->timHw->channelIndex], 4, packet);
 490                 motors[index].requestTelemetry = false;
 491             }
 492         }
 493
 494         for (int burstDmaTimerIndex = 0; burstDmaTimerIndex < burstDmaTimersCount; burstDmaTimerIndex++) {
 495             burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex];
 496             pwmBurstDMAStart(burstDmaTimer, DSHOT_DMA_BUFFER_SIZE * 4);
 497         }
 498 #else
 499         // Generate DMA buffers
 500         for (int index = 0; index < motorCount; index++) {
 501             if (motors[index].pwmPort && motors[index].pwmPort->configured) {
 502                 uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry);
 503                 loadDmaBufferDshot(motors[index].pwmPort->dmaBuffer, packet);
 504                 timerPWMPrepareDMA(motors[index].pwmPort->tch, DSHOT_DMA_BUFFER_SIZE);
 505                 motors[index].requestTelemetry = false;
 506             }
 507         }
 508
 509         // Start DMA on all timers
 510         for (int index = 0; index < motorCount; index++) {
 511             if (motors[index].pwmPort && motors[index].pwmPort->configured) {
 512                 timerPWMStartDMA(motors[index].pwmPort->tch);
 513             }
 514         }
 515 #endif
 516     }
 517 #endif
 518 }
 519
 520 #else // digital motor protocol
 521
 522 // This stub is needed to avoid ESC_SENSOR dependency on DSHOT
 523 void pwmRequestMotorTelemetry(int motorIndex)
 524 {
 525     UNUSED(motorIndex);
 526 }
 527
 528 #endif
 529
 530 void pwmMotorPreconfigure(void)
 531 {
 532     // Keep track of initial motor protocol
 533     initMotorProtocol = motorConfig()->motorPwmProtocol;
 534
 535 #ifdef BRUSHED_MOTORS
 536     initMotorProtocol = PWM_TYPE_BRUSHED;   // Override proto
 537 #endif
 538
 539     // Protocol-specific configuration
 540     switch (initMotorProtocol) {
 541         default:
 542             motorWritePtr = pwmWriteNull;
 543             break;
 544
 545         case PWM_TYPE_STANDARD:
 546         case PWM_TYPE_BRUSHED:
 547         case PWM_TYPE_ONESHOT125:
 548         case PWM_TYPE_MULTISHOT:
 549             motorWritePtr = pwmWriteStandard;
 550             break;
 551
 552 #ifdef USE_DSHOT
 553         case PWM_TYPE_DSHOT600:
 554         case PWM_TYPE_DSHOT300:
 555         case PWM_TYPE_DSHOT150:
 556             motorConfigDigitalUpdateInterval(getEscUpdateFrequency());
 557             motorWritePtr = pwmWriteDigital;
 558             break;
 559 #endif
 560     }
 561 }
 562
 563 /**
 564  * This function return the PWM frequency based on ESC protocol. We allow customer rates only for Brushed motors
 565  */
 566 uint32_t getEscUpdateFrequency(void) {
 567     switch (initMotorProtocol) {
 568         case PWM_TYPE_BRUSHED:
 569             return motorConfig()->motorPwmRate;
 570
 571         case PWM_TYPE_STANDARD:
 572             return 400;
 573
 574         case PWM_TYPE_MULTISHOT:
 575             return 2000;
 576
 577         case PWM_TYPE_DSHOT150:
 578             return 4000;
 579
 580         case PWM_TYPE_DSHOT300:
 581             return 8000;
 582
 583         case PWM_TYPE_DSHOT600:
 584             return 16000;
 585
 586         case PWM_TYPE_ONESHOT125:
 587         default:
 588             return 1000;
 589
 590     }
 591 }
 592
 593 bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bool enableOutput)
 594 {
 595     switch (initMotorProtocol) {
 596     case PWM_TYPE_BRUSHED:
 597         motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 0.0f, 0.0f, getEscUpdateFrequency(), enableOutput);
 598         break;
 599
 600     case PWM_TYPE_ONESHOT125:
 601         motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 125e-6f, 125e-6f, getEscUpdateFrequency(), enableOutput);
 602         break;
 603
 604     case PWM_TYPE_MULTISHOT:
 605         motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 5e-6f, 20e-6f, getEscUpdateFrequency(), enableOutput);
 606         break;
 607
 608 #ifdef USE_DSHOT
 609     case PWM_TYPE_DSHOT600:
 610     case PWM_TYPE_DSHOT300:
 611     case PWM_TYPE_DSHOT150:
 612         motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput);
 613         break;
 614 #endif
 615
 616     case PWM_TYPE_STANDARD:
 617         motors[motorIndex].pwmPort = motorConfigPwm(timerHardware, 1e-3f, 1e-3f, getEscUpdateFrequency(), enableOutput);
 618         break;
 619
 620     default:
 621         motors[motorIndex].pwmPort = NULL;
 622         break;
 623     }
 624
 625     return (motors[motorIndex].pwmPort != NULL);
 626 }
 627
 628 // Helper function for ESC passthrough
 629 ioTag_t pwmGetMotorPinTag(int motorIndex)
 630 {
 631     if (motors[motorIndex].pwmPort) {
 632         return motors[motorIndex].pwmPort->tch->timHw->tag;
 633     }
 634     else {
 635         return IOTAG_NONE;
 636     }
 637 }
 638
 639 static void pwmServoWriteStandard(uint8_t index, uint16_t value)
 640 {
 641     if (index < MAX_SERVOS && servos[index]) {
 642         *servos[index]->ccr = value;
 643     }
 644 }
 645
 646 #ifdef USE_SERVO_SBUS
 647 static void sbusPwmWriteStandard(uint8_t index, uint16_t value)
 648 {
 649     pwmServoWriteStandard(index, value);
 650     sbusServoUpdate(index, value);
 651 }
 652 #endif
 653
 654 void pwmServoPreconfigure(void)
 655 {
 656     // Protocol-specific configuration
 657     switch (servoConfig()->servo_protocol) {
 658         default:
 659         case SERVO_TYPE_PWM:
 660             servoWritePtr = pwmServoWriteStandard;
 661             break;
 662
 663 #ifdef USE_SERVO_SBUS
 664         case SERVO_TYPE_SBUS:
 665             sbusServoInitialize();
 666             servoWritePtr = sbusServoUpdate;
 667             break;
 668
 669         case SERVO_TYPE_SBUS_PWM:
 670             sbusServoInitialize();
 671             servoWritePtr = sbusPwmWriteStandard;
 672             break;
 673 #endif
 674     }
 675 }
 676
 677 bool pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput)
 678 {
 679     pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_SERVO, PWM_TIMER_HZ, PWM_TIMER_HZ / servoPwmRate, servoCenterPulse, enableOutput);
 680
 681     if (port) {
 682         servos[servoIndex] = port;
 683         return true;
 684     }
 685
 686     return false;
 687 }
 688
 689 void pwmWriteServo(uint8_t index, uint16_t value)
 690 {
 691     if (servoWritePtr && index < MAX_SERVOS) {
 692         servoWritePtr(index, value);
 693     }
 694 }
 695
 696 void pwmWriteBeeper(bool onoffBeep)
 697 {
 698     if (beeperPwm == NULL)
 699         return;
 700
 701     if (onoffBeep == true) {
 702         *beeperPwm->ccr = (1000000 / beeperFrequency) / 2;
 703     } else {
 704         *beeperPwm->ccr = 0;
 705     }
 706 }
 707
 708 void beeperPwmInit(ioTag_t tag, uint16_t frequency)
 709 {
 710     beeperPwm = NULL;
 711
 712     const timerHardware_t *timHw = timerGetByTag(tag, TIM_USE_BEEPER);
 713
 714     if (timHw) {
 715         // Attempt to allocate TCH
 716         TCH_t * tch = timerGetTCH(timHw);
 717         if (tch == NULL) {
 718             return;
 719         }
 720
 721         beeperPwm = &beeperPwmPort;
 722         beeperFrequency = frequency;
 723         IOConfigGPIOAF(IOGetByTag(tag), IOCFG_AF_PP, timHw->alternateFunction);
 724         pwmOutConfigTimer(beeperPwm, tch, PWM_TIMER_HZ, 1000000 / beeperFrequency, (1000000 / beeperFrequency) / 2);
 725     }
 726 }
 727
 728 #endif