src/platform/APM32/pwm_output_apm32.c

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