src/main/drivers/pwm_output.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_PWM_OUTPUT
  29
  30 #include "drivers/io.h"
  31 #include "drivers/motor.h"
  32 #include "drivers/pwm_output.h"
  33 #include "drivers/time.h"
  34 #include "drivers/timer.h"
  35
  36 #include "pg/motor.h"
  37
  38 FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
  39
  40 static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
  41 {
  42 #if defined(USE_HAL_DRIVER)
  43     TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
  44     if (Handle == NULL) return;
  45
  46     TIM_OC_InitTypeDef TIM_OCInitStructure;
  47
  48     TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
  49     TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
  50     TIM_OCInitStructure.OCPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPOLARITY_LOW : TIM_OCPOLARITY_HIGH;
  51     TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_SET;
  52     TIM_OCInitStructure.OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPOLARITY_LOW : TIM_OCNPOLARITY_HIGH;
  53     TIM_OCInitStructure.Pulse = value;
  54     TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
  55
  56     HAL_TIM_PWM_ConfigChannel(Handle, &TIM_OCInitStructure, channel);
  57 #else
  58     TIM_OCInitTypeDef TIM_OCInitStructure;
  59
  60     TIM_OCStructInit(&TIM_OCInitStructure);
  61     TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  62
  63     if (output & TIMER_OUTPUT_N_CHANNEL) {
  64         TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
  65         TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
  66         TIM_OCInitStructure.TIM_OCNPolarity = (output & TIMER_OUTPUT_INVERTED) ? TIM_OCNPolarity_Low : TIM_OCNPolarity_High;
  67     } else {
  68         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  69         TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
  70         TIM_OCInitStructure.TIM_OCPolarity =  (output & TIMER_OUTPUT_INVERTED) ? TIM_OCPolarity_Low : TIM_OCPolarity_High;
  71     }
  72     TIM_OCInitStructure.TIM_Pulse = value;
  73
  74     timerOCInit(tim, channel, &TIM_OCInitStructure);
  75     timerOCPreloadConfig(tim, channel, TIM_OCPreload_Enable);
  76 #endif
  77 }
  78
  79 void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
  80 {
  81 #if defined(USE_HAL_DRIVER)
  82     TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
  83     if (Handle == NULL) return;
  84 #endif
  85
  86     configTimeBase(timerHardware->tim, period, hz);
  87     pwmOCConfig(timerHardware->tim,
  88         timerHardware->channel,
  89         value,
  90         inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
  91         );
  92
  93 #if defined(USE_HAL_DRIVER)
  94     if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
  95         HAL_TIMEx_PWMN_Start(Handle, timerHardware->channel);
  96     else
  97         HAL_TIM_PWM_Start(Handle, timerHardware->channel);
  98     HAL_TIM_Base_Start(Handle);
  99 #else
 100     TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE);
 101     TIM_Cmd(timerHardware->tim, ENABLE);
 102 #endif
 103
 104     channel->ccr = timerChCCR(timerHardware);
 105
 106     channel->tim = timerHardware->tim;
 107
 108     *channel->ccr = 0;
 109 }
 110
 111 static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice;
 112
 113 static void pwmWriteUnused(uint8_t index, float value)
 114 {
 115     UNUSED(index);
 116     UNUSED(value);
 117 }
 118
 119 static void pwmWriteStandard(uint8_t index, float value)
 120 {
 121     /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
 122     *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
 123 }
 124
 125 void pwmShutdownPulsesForAllMotors(void)
 126 {
 127     for (int index = 0; index < motorPwmDevice.count; index++) {
 128         // Set the compare register to 0, which stops the output pulsing if the timer overflows
 129         if (motors[index].channel.ccr) {
 130             *motors[index].channel.ccr = 0;
 131         }
 132     }
 133 }
 134
 135 void pwmDisableMotors(void)
 136 {
 137     pwmShutdownPulsesForAllMotors();
 138 }
 139
 140 static motorVTable_t motorPwmVTable;
 141 bool pwmEnableMotors(void)
 142 {
 143     /* check motors can be enabled */
 144     return (motorPwmVTable.write != &pwmWriteUnused);
 145 }
 146
 147 bool pwmIsMotorEnabled(uint8_t index)
 148 {
 149     return motors[index].enabled;
 150 }
 151
 152 static void pwmCompleteOneshotMotorUpdate(void)
 153 {
 154     for (int index = 0; index < motorPwmDevice.count; index++) {
 155         if (motors[index].forceOverflow) {
 156             timerForceOverflow(motors[index].channel.tim);
 157         }
 158         // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
 159         // This compare register will be set to the output value on the next main loop.
 160         *motors[index].channel.ccr = 0;
 161     }
 162 }
 163
 164 static float pwmConvertFromExternal(uint16_t externalValue)
 165 {
 166     return (float)externalValue;
 167 }
 168
 169 static uint16_t pwmConvertToExternal(float motorValue)
 170 {
 171     return (uint16_t)motorValue;
 172 }
 173
 174 static motorVTable_t motorPwmVTable = {
 175     .postInit = motorPostInitNull,
 176     .enable = pwmEnableMotors,
 177     .disable = pwmDisableMotors,
 178     .isMotorEnabled = pwmIsMotorEnabled,
 179     .shutdown = pwmShutdownPulsesForAllMotors,
 180     .convertExternalToMotor = pwmConvertFromExternal,
 181     .convertMotorToExternal = pwmConvertToExternal,
 182 };
 183
 184 motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
 185 {
 186     motorPwmDevice.vTable = motorPwmVTable;
 187
 188     float sMin = 0;
 189     float sLen = 0;
 190     switch (motorConfig->motorPwmProtocol) {
 191     default:
 192     case PWM_TYPE_ONESHOT125:
 193         sMin = 125e-6f;
 194         sLen = 125e-6f;
 195         break;
 196     case PWM_TYPE_ONESHOT42:
 197         sMin = 42e-6f;
 198         sLen = 42e-6f;
 199         break;
 200     case PWM_TYPE_MULTISHOT:
 201         sMin = 5e-6f;
 202         sLen = 20e-6f;
 203         break;
 204     case PWM_TYPE_BRUSHED:
 205         sMin = 0;
 206         useUnsyncedPwm = true;
 207         idlePulse = 0;
 208         break;
 209     case PWM_TYPE_STANDARD:
 210         sMin = 1e-3f;
 211         sLen = 1e-3f;
 212         useUnsyncedPwm = true;
 213         idlePulse = 0;
 214         break;
 215     }
 216
 217     motorPwmDevice.vTable.write = pwmWriteStandard;
 218     motorPwmDevice.vTable.updateStart = motorUpdateStartNull;
 219     motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
 220
 221     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 222         const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
 223         const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
 224         const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 225
 226         if (timerHardware == NULL) {
 227             /* not enough motors initialised for the mixer or a break in the motors */
 228             motorPwmDevice.vTable.write = &pwmWriteUnused;
 229             motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
 230             /* TODO: block arming and add reason system cannot arm */
 231             return NULL;
 232         }
 233
 234         motors[motorIndex].io = IOGetByTag(tag);
 235         IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 236
 237 #if defined(STM32F1)
 238         IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
 239 #else
 240         IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
 241 #endif
 242
 243         /* standard PWM outputs */
 244         // margin of safety is 4 periods when unsynced
 245         const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
 246
 247         const uint32_t clock = timerClock(timerHardware->tim);
 248         /* used to find the desired timer frequency for max resolution */
 249         const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
 250         const uint32_t hz = clock / prescaler;
 251         const unsigned period = useUnsyncedPwm ? hz / pwmRateHz : 0xffff;
 252
 253         /*
 254             if brushed then it is the entire length of the period.
 255             TODO: this can be moved back to periodMin and periodLen
 256             once mixer outputs a 0..1 float value.
 257         */
 258         motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
 259         motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
 260
 261         pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
 262
 263         bool timerAlreadyUsed = false;
 264         for (int i = 0; i < motorIndex; i++) {
 265             if (motors[i].channel.tim == motors[motorIndex].channel.tim) {
 266                 timerAlreadyUsed = true;
 267                 break;
 268             }
 269         }
 270         motors[motorIndex].forceOverflow = !timerAlreadyUsed;
 271         motors[motorIndex].enabled = true;
 272     }
 273
 274     return &motorPwmDevice;
 275 }
 276
 277 pwmOutputPort_t *pwmGetMotors(void)
 278 {
 279     return motors;
 280 }
 281
 282 #ifdef USE_SERVOS
 283 static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
 284
 285 void pwmWriteServo(uint8_t index, float value)
 286 {
 287     if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) {
 288         *servos[index].channel.ccr = lrintf(value);
 289     }
 290 }
 291
 292 void servoDevInit(const servoDevConfig_t *servoConfig)
 293 {
 294     for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
 295         const ioTag_t tag = servoConfig->ioTags[servoIndex];
 296
 297         if (!tag) {
 298             break;
 299         }
 300
 301         servos[servoIndex].io = IOGetByTag(tag);
 302
 303         IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
 304
 305         const timerHardware_t *timer = timerAllocate(tag, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
 306
 307         if (timer == NULL) {
 308             /* flag failure and disable ability to arm */
 309             break;
 310         }
 311
 312 #if defined(STM32F1)
 313         IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
 314 #else
 315         IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
 316 #endif
 317
 318         pwmOutConfig(&servos[servoIndex].channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
 319         servos[servoIndex].enabled = true;
 320     }
 321 }
 322 #endif // USE_SERVOS
 323 #endif // USE_PWM_OUTPUT