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