src/main/drivers/pwm_mapping.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
  22 #include "platform.h"
  23
  24 #if !defined(SITL_BUILD)
  25
  26 #include "build/debug.h"
  27 #include "common/log.h"
  28 #include "common/memory.h"
  29
  30 #include "config/feature.h"
  31
  32 #include "fc/config.h"
  33
  34 #include "drivers/io.h"
  35 #include "drivers/io_impl.h"
  36 #include "drivers/timer.h"
  37 #include "drivers/pwm_output.h"
  38 #include "drivers/pwm_mapping.h"
  39 #include "drivers/serial.h"
  40 #include "drivers/serial_uart.h"
  41
  42 #include "sensors/rangefinder.h"
  43
  44 #include "io/serial.h"
  45 #include "io/servo_sbus.h"
  46
  47 enum {
  48     MAP_TO_NONE,
  49     MAP_TO_MOTOR_OUTPUT,
  50     MAP_TO_SERVO_OUTPUT,
  51     MAP_TO_LED_OUTPUT
  52 };
  53
  54 typedef struct {
  55     int maxTimMotorCount;
  56     int maxTimServoCount;
  57     const timerHardware_t * timMotors[MAX_PWM_OUTPUTS];
  58     const timerHardware_t * timServos[MAX_PWM_OUTPUTS];
  59 } timMotorServoHardware_t;
  60
  61 static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE;
  62
  63 static const char * pwmInitErrorMsg[] = {
  64     /* PWM_INIT_ERROR_NONE */                     "No error",
  65     /* PWM_INIT_ERROR_TOO_MANY_MOTORS */          "Mixer defines too many motors",
  66     /* PWM_INIT_ERROR_TOO_MANY_SERVOS */          "Mixer defines too many servos",
  67     /* PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS */ "Not enough motor outputs/timers",
  68     /* PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS */ "Not enough servo outputs/timers",
  69     /* PWM_INIT_ERROR_TIMER_INIT_FAILED */        "Output timer init failed"
  70 };
  71
  72 static const motorProtocolProperties_t motorProtocolProperties[] = {
  73     [PWM_TYPE_STANDARD]     = { .usesHwTimer = true,    .isDSHOT = false },
  74     [PWM_TYPE_ONESHOT125]   = { .usesHwTimer = true,    .isDSHOT = false },
  75     [PWM_TYPE_MULTISHOT]    = { .usesHwTimer = true,    .isDSHOT = false },
  76     [PWM_TYPE_BRUSHED]      = { .usesHwTimer = true,    .isDSHOT = false },
  77     [PWM_TYPE_DSHOT150]     = { .usesHwTimer = true,    .isDSHOT = true },
  78     [PWM_TYPE_DSHOT300]     = { .usesHwTimer = true,    .isDSHOT = true },
  79     [PWM_TYPE_DSHOT600]     = { .usesHwTimer = true,    .isDSHOT = true },
  80 };
  81
  82 pwmInitError_e getPwmInitError(void)
  83 {
  84     return pwmInitError;
  85 }
  86
  87 const char * getPwmInitErrorMessage(void)
  88 {
  89     return pwmInitErrorMsg[pwmInitError];
  90 }
  91
  92 const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto)
  93 {
  94     return &motorProtocolProperties[proto];
  95 }
  96
  97 static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
  98 {
  99     serialPortPins_t uartPins;
 100
 101 #if defined(USE_UART2)
 102     uartGetPortPins(UARTDEV_2, &uartPins);
 103     if (doesConfigurationUsePort(SERIAL_PORT_USART2) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) {
 104         return true;
 105     }
 106 #endif
 107
 108 #if defined(USE_UART3)
 109     uartGetPortPins(UARTDEV_3, &uartPins);
 110     if (doesConfigurationUsePort(SERIAL_PORT_USART3) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) {
 111         return true;
 112     }
 113 #endif
 114
 115 #if defined(USE_UART4)
 116     uartGetPortPins(UARTDEV_4, &uartPins);
 117     if (doesConfigurationUsePort(SERIAL_PORT_USART4) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) {
 118         return true;
 119     }
 120 #endif
 121
 122 #if defined(USE_UART5)
 123     uartGetPortPins(UARTDEV_5, &uartPins);
 124     if (doesConfigurationUsePort(SERIAL_PORT_USART5) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) {
 125         return true;
 126     }
 127 #endif
 128
 129 #if defined(USE_UART6)
 130     uartGetPortPins(UARTDEV_6, &uartPins);
 131     if (doesConfigurationUsePort(SERIAL_PORT_USART6) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) {
 132         return true;
 133     }
 134 #endif
 135
 136 #if defined(USE_UART7)
 137     uartGetPortPins(UARTDEV_7, &uartPins);
 138     if (doesConfigurationUsePort(SERIAL_PORT_USART7) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) {
 139         return true;
 140     }
 141 #endif
 142
 143 #if defined(USE_UART8)
 144     uartGetPortPins(UARTDEV_8, &uartPins);
 145     if (doesConfigurationUsePort(SERIAL_PORT_USART8) && (timHw->tag == uartPins.txPin || timHw->tag == uartPins.rxPin)) {
 146         return true;
 147     }
 148 #endif
 149
 150 #if defined(USE_SOFTSERIAL1)
 151     if (feature(FEATURE_SOFTSERIAL)) {
 152         const timerHardware_t *ssrx = timerGetByTag(IO_TAG(SOFTSERIAL_1_RX_PIN), TIM_USE_ANY);
 153         const timerHardware_t *sstx = timerGetByTag(IO_TAG(SOFTSERIAL_1_TX_PIN), TIM_USE_ANY);
 154         if ((ssrx != NULL && ssrx->tim == timHw->tim) || (sstx != NULL && sstx->tim == timHw->tim)) {
 155             return true;
 156         }
 157     }
 158 #endif
 159
 160 #if defined(USE_SOFTSERIAL2)
 161     if (feature(FEATURE_SOFTSERIAL)) {
 162         const timerHardware_t *ssrx = timerGetByTag(IO_TAG(SOFTSERIAL_2_RX_PIN), TIM_USE_ANY);
 163         const timerHardware_t *sstx = timerGetByTag(IO_TAG(SOFTSERIAL_2_TX_PIN), TIM_USE_ANY);
 164         if ((ssrx != NULL && ssrx->tim == timHw->tim) || (sstx != NULL && sstx->tim == timHw->tim)) {
 165             return true;
 166         }
 167     }
 168 #endif
 169
 170 #if defined(USE_LED_STRIP)
 171     if (feature(FEATURE_LED_STRIP)) {
 172         for (int i = 0; i < timerHardwareCount; i++) {
 173             if (timHw->tim == timerHardware[i].tim && timerHardware[i].usageFlags & TIM_USE_LED) {
 174                                 return true;
 175             }
 176         }
 177
 178         //const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY);
 179         //if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) {
 180         //    return true;
 181         //}
 182     }
 183 #endif
 184
 185 #if defined(USE_ADC)
 186 #if defined(ADC_CHANNEL_1_PIN)
 187     if (timHw->tag == IO_TAG(ADC_CHANNEL_1_PIN)) {
 188         return true;
 189     }
 190 #endif
 191 #if defined(ADC_CHANNEL_2_PIN)
 192     if (timHw->tag == IO_TAG(ADC_CHANNEL_2_PIN)) {
 193         return true;
 194     }
 195 #endif
 196 #if defined(ADC_CHANNEL_3_PIN)
 197     if (timHw->tag == IO_TAG(ADC_CHANNEL_3_PIN)) {
 198         return true;
 199     }
 200 #endif
 201 #if defined(ADC_CHANNEL_4_PIN)
 202     if (timHw->tag == IO_TAG(ADC_CHANNEL_4_PIN)) {
 203         return true;
 204     }
 205 #endif
 206 #if defined(ADC_CHANNEL_5_PIN)
 207     if (timHw->tag == IO_TAG(ADC_CHANNEL_5_PIN)) {
 208         return true;
 209     }
 210 #endif
 211 #if defined(ADC_CHANNEL_6_PIN)
 212     if (timHw->tag == IO_TAG(ADC_CHANNEL_6_PIN)) {
 213         return true;
 214     }
 215 #endif
 216 #endif
 217
 218     return false;
 219 }
 220
 221 static void timerHardwareOverride(timerHardware_t * timer) {
 222     switch (timerOverrides(timer2id(timer->tim))->outputMode) {
 223         case OUTPUT_MODE_MOTORS:
 224             timer->usageFlags &= ~(TIM_USE_SERVO|TIM_USE_LED);
 225             timer->usageFlags |= TIM_USE_MOTOR;
 226             break;
 227         case OUTPUT_MODE_SERVOS:
 228             timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_LED);
 229             timer->usageFlags |= TIM_USE_SERVO;
 230             break;
 231         case OUTPUT_MODE_LED:
 232             timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_SERVO);
 233             timer->usageFlags |= TIM_USE_LED;
 234             break;
 235     }
 236 }
 237
 238 bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim)
 239 {
 240     for (int i = 0; i < timOutputs->maxTimMotorCount; ++i) {
 241         if (timOutputs->timMotors[i]->tim == tim) {
 242             return true;
 243         }
 244     }
 245
 246     return false;
 247 }
 248
 249 bool pwmHasServoOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim)
 250 {
 251     for (int i = 0; i < timOutputs->maxTimServoCount; ++i) {
 252         if (timOutputs->timServos[i]->tim == tim) {
 253             return true;
 254         }
 255     }
 256
 257     return false;
 258 }
 259
 260 uint8_t pwmClaimTimer(HAL_Timer_t *tim, uint32_t usageFlags) {
 261     uint8_t changed = 0;
 262     for (int idx = 0; idx < timerHardwareCount; idx++) {
 263         timerHardware_t *timHw = &timerHardware[idx];
 264         if (timHw->tim == tim && timHw->usageFlags != usageFlags) {
 265             timHw->usageFlags = usageFlags;
 266             changed++;
 267         }
 268     }
 269
 270     return changed;
 271 }
 272
 273 void pwmEnsureEnoughtMotors(uint8_t motorCount)
 274 {
 275     uint8_t motorOnlyOutputs = 0;
 276
 277     for (int idx = 0; idx < timerHardwareCount; idx++) {
 278         timerHardware_t *timHw = &timerHardware[idx];
 279
 280         timerHardwareOverride(timHw);
 281
 282         if (checkPwmTimerConflicts(timHw)) {
 283             continue;
 284         }
 285
 286         if (TIM_IS_MOTOR_ONLY(timHw->usageFlags)) {
 287             motorOnlyOutputs++;
 288             motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
 289         }
 290     }
 291
 292     for (int idx = 0; idx < timerHardwareCount; idx++) {
 293         timerHardware_t *timHw = &timerHardware[idx];
 294
 295         if (checkPwmTimerConflicts(timHw)) {
 296             continue;
 297         }
 298
 299         if (TIM_IS_MOTOR(timHw->usageFlags) && !TIM_IS_MOTOR_ONLY(timHw->usageFlags)) {
 300             if (motorOnlyOutputs < motorCount) {
 301                 timHw->usageFlags &= ~TIM_USE_SERVO;
 302                 timHw->usageFlags |= TIM_USE_MOTOR;
 303                 motorOnlyOutputs++;
 304                 motorOnlyOutputs += pwmClaimTimer(timHw->tim, timHw->usageFlags);
 305             } else {
 306                 timHw->usageFlags &= ~TIM_USE_MOTOR;
 307                 pwmClaimTimer(timHw->tim, timHw->usageFlags);
 308             }
 309         }
 310     }
 311 }
 312
 313 void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos)
 314 {
 315     UNUSED(isMixerUsingServos);
 316     timOutputs->maxTimMotorCount = 0;
 317     timOutputs->maxTimServoCount = 0;
 318
 319     uint8_t motorCount = getMotorCount();
 320     uint8_t motorIdx = 0;
 321
 322     pwmEnsureEnoughtMotors(motorCount);
 323
 324     for (int idx = 0; idx < timerHardwareCount; idx++) {
 325         timerHardware_t *timHw = &timerHardware[idx];
 326
 327         int type = MAP_TO_NONE;
 328
 329         // Check for known conflicts (i.e. UART, LEDSTRIP, Rangefinder and ADC)
 330         if (checkPwmTimerConflicts(timHw)) {
 331             LOG_WARNING(PWM, "Timer output %d skipped", idx);
 332             continue;
 333         }
 334
 335         // Make sure first motorCount motor outputs get assigned to motor
 336         if (TIM_IS_MOTOR(timHw->usageFlags) && (motorIdx < motorCount)) {
 337             timHw->usageFlags &= ~TIM_USE_SERVO;
 338             pwmClaimTimer(timHw->tim, timHw->usageFlags);
 339             motorIdx += 1;
 340         }
 341
 342         if (TIM_IS_SERVO(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim)) {
 343             type = MAP_TO_SERVO_OUTPUT;
 344         } else if (TIM_IS_MOTOR(timHw->usageFlags) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
 345             type = MAP_TO_MOTOR_OUTPUT;
 346         } else if (TIM_IS_LED(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) {
 347             type = MAP_TO_LED_OUTPUT;
 348         }
 349
 350         switch(type) {
 351             case MAP_TO_MOTOR_OUTPUT:
 352                 timHw->usageFlags &= TIM_USE_MOTOR;
 353                 timOutputs->timMotors[timOutputs->maxTimMotorCount++] = timHw;
 354                 pwmClaimTimer(timHw->tim, timHw->usageFlags);
 355                 break;
 356             case MAP_TO_SERVO_OUTPUT:
 357                 timHw->usageFlags &= TIM_USE_SERVO;
 358                 timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw;
 359                 pwmClaimTimer(timHw->tim, timHw->usageFlags);
 360                 break;
 361             case MAP_TO_LED_OUTPUT:
 362                 timHw->usageFlags &= TIM_USE_LED;
 363                 pwmClaimTimer(timHw->tim, timHw->usageFlags);
 364                 break;
 365             default:
 366                 break;
 367         }
 368     }
 369 }
 370
 371 static bool motorsUseHardwareTimers(void)
 372 {
 373     return getMotorProtocolProperties(motorConfig()->motorPwmProtocol)->usesHwTimer;
 374 }
 375
 376 static bool servosUseHardwareTimers(void)
 377 {
 378     return servoConfig()->servo_protocol == SERVO_TYPE_PWM ||
 379         servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM;
 380 }
 381
 382 static void pwmInitMotors(timMotorServoHardware_t * timOutputs)
 383 {
 384     const int motorCount = getMotorCount();
 385
 386     // Check if too many motors
 387     if (motorCount > MAX_MOTORS) {
 388         pwmInitError = PWM_INIT_ERROR_TOO_MANY_MOTORS;
 389         LOG_ERROR(PWM, "Too many motors. Mixer requested %d, max %d", motorCount, MAX_MOTORS);
 390         return;
 391     }
 392
 393     // Do the pre-configuration. For motors w/o hardware timers this should be sufficient
 394     pwmMotorPreconfigure();
 395
 396     // Now if we need to configure individual motor outputs - do that
 397     if (!motorsUseHardwareTimers()) {
 398         LOG_INFO(PWM, "Skipped timer init for motors");
 399         return;
 400     }
 401
 402     // If mixer requests more motors than we have timer outputs - throw an error
 403     if (motorCount > timOutputs->maxTimMotorCount) {
 404         pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS;
 405         LOG_ERROR(PWM, "Not enough motor outputs. Mixer requested %d, outputs %d", motorCount, timOutputs->maxTimMotorCount);
 406         return;
 407     }
 408
 409     // Finally initialize individual motor outputs
 410     for (int idx = 0; idx < motorCount; idx++) {
 411         const timerHardware_t *timHw = timOutputs->timMotors[idx];
 412         if (!pwmMotorConfig(timHw, idx, feature(FEATURE_PWM_OUTPUT_ENABLE))) {
 413             pwmInitError = PWM_INIT_ERROR_TIMER_INIT_FAILED;
 414             LOG_ERROR(PWM, "Timer allocation failed for motor %d", idx);
 415             return;
 416         }
 417     }
 418 }
 419
 420 static void pwmInitServos(timMotorServoHardware_t * timOutputs)
 421 {
 422     const int servoCount = getServoCount();
 423
 424     if (!isMixerUsingServos()) {
 425         LOG_INFO(PWM, "Mixer does not use servos");
 426         return;
 427     }
 428
 429     // Check if too many servos
 430     if (servoCount > MAX_SERVOS) {
 431         pwmInitError = PWM_INIT_ERROR_TOO_MANY_SERVOS;
 432         LOG_ERROR(PWM, "Too many servos. Mixer requested %d, max %d", servoCount, MAX_SERVOS);
 433         return;
 434     }
 435
 436     // Do the pre-configuration. This should configure non-timer PWM drivers
 437     pwmServoPreconfigure();
 438
 439     // Check if we need to init timer output for servos
 440     if (!servosUseHardwareTimers()) {
 441         // External PWM servo driver
 442         LOG_INFO(PWM, "Skipped timer init for servos - using external servo driver");
 443         return;
 444     }
 445
 446
 447     // If mixer requests more servos than we have timer outputs - throw an error
 448     uint16_t maxServos = timOutputs->maxTimServoCount;
 449     if (servoConfig()->servo_protocol == SERVO_TYPE_SBUS_PWM) {
 450         maxServos = MAX(SERVO_SBUS_MAX_SERVOS, timOutputs->maxTimServoCount);
 451     }
 452
 453     if (servoCount > maxServos) {
 454         pwmInitError = PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS;
 455         LOG_ERROR(PWM, "Too many servos. Mixer requested %d, timer outputs %d", servoCount, timOutputs->maxTimServoCount);
 456         return;
 457     }
 458
 459     // Configure individual servo outputs
 460     for (int idx = 0; idx < MIN(servoCount, timOutputs->maxTimServoCount); idx++) {
 461         const timerHardware_t *timHw = timOutputs->timServos[idx];
 462
 463         if (!pwmServoConfig(timHw, idx, servoConfig()->servoPwmRate, servoConfig()->servoCenterPulse, feature(FEATURE_PWM_OUTPUT_ENABLE))) {
 464             pwmInitError = PWM_INIT_ERROR_TIMER_INIT_FAILED;
 465             LOG_ERROR(PWM, "Timer allocation failed for servo %d", idx);
 466             return;
 467         }
 468     }
 469 }
 470
 471
 472 bool pwmMotorAndServoInit(void)
 473 {
 474     timMotorServoHardware_t timOutputs;
 475
 476     // Build temporary timer mappings for motor and servo
 477     pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos());
 478
 479     // At this point we have built tables of timers suitable for motor and servo mappings
 480     // Now we can actually initialize them according to motor/servo count from mixer
 481     pwmInitMotors(&timOutputs);
 482     pwmInitServos(&timOutputs);
 483
 484     return (pwmInitError == PWM_INIT_ERROR_NONE);
 485 }
 486
 487 #endif