src/platform/AT32/pwm_output_at32bsp.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_impl.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 static void pwmOCConfig(tmr_type *tim, uint8_t channel, uint16_t value, uint8_t output)
  40 {
  41     tmr_output_config_type  tmr_OCInitStruct;
  42     tmr_output_default_para_init(&tmr_OCInitStruct);
  43     tmr_OCInitStruct.oc_mode= TMR_OUTPUT_CONTROL_PWM_MODE_A;
  44
  45     if (output & TIMER_OUTPUT_N_CHANNEL) {
  46         tmr_OCInitStruct.occ_output_state = TRUE;
  47         tmr_OCInitStruct.occ_idle_state = FALSE;
  48         tmr_OCInitStruct.occ_polarity =  (output & TIMER_OUTPUT_INVERTED) ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH;
  49     } else {
  50         tmr_OCInitStruct.oc_output_state = TRUE;
  51         tmr_OCInitStruct.oc_idle_state = TRUE;
  52         tmr_OCInitStruct.oc_polarity =  (output & TIMER_OUTPUT_INVERTED) ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH;
  53     }
  54     tmr_channel_value_set(tim, (channel-1)*2, value);
  55     tmr_output_channel_config(tim,(channel-1)*2, &tmr_OCInitStruct);
  56     tmr_output_channel_buffer_enable(tim, ((channel-1)*2),TRUE);
  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     configTimeBase(timerHardware->tim, period, hz);
  62     pwmOCConfig(timerHardware->tim,
  63         timerHardware->channel,
  64         value,
  65         inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
  66         );
  67
  68     tmr_output_enable(timerHardware->tim, TRUE);
  69     tmr_counter_enable(timerHardware->tim, TRUE);
  70
  71     channel->ccr = timerChCCR(timerHardware);
  72
  73     channel->tim = timerHardware->tim;
  74
  75     *channel->ccr = 0;
  76 }
  77
  78 static FAST_DATA_ZERO_INIT motorDevice_t *pwmMotorDevice;
  79
  80 static void pwmWriteStandard(uint8_t index, float value)
  81 {
  82     /* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
  83     *motors[index].channel.ccr = lrintf((value * motors[index].pulseScale) + motors[index].pulseOffset);
  84 }
  85
  86 static void pwmShutdownPulsesForAllMotors(void)
  87 {
  88     for (int index = 0; index < pwmMotorCount; index++) {
  89         // Set the compare register to 0, which stops the output pulsing if the timer overflows
  90         if (motors[index].channel.ccr) {
  91             *motors[index].channel.ccr = 0;
  92         }
  93     }
  94 }
  95
  96 static void pwmDisableMotors(void)
  97 {
  98     pwmShutdownPulsesForAllMotors();
  99 }
 100
 101 static motorVTable_t motorPwmVTable;
 102 static bool pwmEnableMotors(void)
 103 {
 104     /* check motors can be enabled */
 105     return (pwmMotorDevice->vTable);
 106 }
 107
 108 static bool pwmIsMotorEnabled(unsigned index)
 109 {
 110     return motors[index].enabled;
 111 }
 112
 113 static bool useContinuousUpdate = true;
 114
 115 static void pwmCompleteMotorUpdate(void)
 116 {
 117     if (useContinuousUpdate) {
 118         return;
 119     }
 120
 121     for (int index = 0; index < pwmMotorCount; index++) {
 122         if (motors[index].forceOverflow) {
 123             timerForceOverflow(motors[index].channel.tim);
 124         }
 125         // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
 126         // This compare register will be set to the output value on the next main loop.
 127         *motors[index].channel.ccr = 0;
 128     }
 129 }
 130
 131 static float pwmConvertFromExternal(uint16_t externalValue)
 132 {
 133     return (float)externalValue;
 134 }
 135
 136 static uint16_t pwmConvertToExternal(float motorValue)
 137 {
 138     return (uint16_t)motorValue;
 139 }
 140
 141 static motorVTable_t motorPwmVTable = {
 142     .postInit = motorPostInitNull,
 143     .enable = pwmEnableMotors,
 144     .disable = pwmDisableMotors,
 145     .isMotorEnabled = pwmIsMotorEnabled,
 146     .shutdown = pwmShutdownPulsesForAllMotors,
 147     .convertExternalToMotor = pwmConvertFromExternal,
 148     .convertMotorToExternal = pwmConvertToExternal,
 149     .write = pwmWriteStandard,
 150     .decodeTelemetry = motorDecodeTelemetryNull,
 151     .updateComplete = pwmCompleteMotorUpdate,
 152     .requestTelemetry = NULL,
 153     .isMotorIdle = NULL,
 154 };
 155
 156 bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)
 157 {
 158     memset(motors, 0, sizeof(motors));
 159
 160     if (!device) {
 161         return false;
 162     }
 163
 164     device->vTable = &motorPwmVTable;
 165     pwmMotorDevice = device;
 166     pwmMotorCount = device->count;
 167     useContinuousUpdate = motorConfig->useContinuousUpdate;
 168
 169     float sMin = 0;
 170     float sLen = 0;
 171     switch (motorConfig->motorProtocol) {
 172     default:
 173     case MOTOR_PROTOCOL_ONESHOT125:
 174         sMin = 125e-6f;
 175         sLen = 125e-6f;
 176         break;
 177     case MOTOR_PROTOCOL_ONESHOT42:
 178         sMin = 42e-6f;
 179         sLen = 42e-6f;
 180         break;
 181     case MOTOR_PROTOCOL_MULTISHOT:
 182         sMin = 5e-6f;
 183         sLen = 20e-6f;
 184         break;
 185     case MOTOR_PROTOCOL_BRUSHED:
 186         sMin = 0;
 187         useContinuousUpdate = true;
 188         idlePulse = 0;
 189         break;
 190     case MOTOR_PROTOCOL_PWM :
 191         sMin = 1e-3f;
 192         sLen = 1e-3f;
 193         useContinuousUpdate = true;
 194         idlePulse = 0;
 195         break;
 196     }
 197
 198     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) {
 199         const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
 200         const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
 201         const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 202
 203         if (timerHardware == NULL) {
 204             /* not enough motors initialised for the mixer or a break in the motors */
 205             device->vTable = NULL;
 206             pwmMotorCount = 0;
 207             /* TODO: block arming and add reason system cannot arm */
 208             return false;
 209         }
 210
 211         motors[motorIndex].io = IOGetByTag(tag);
 212         IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 213
 214         IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
 215
 216         /* standard PWM outputs */
 217         // margin of safety is 4 periods when unsynced
 218         const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
 219
 220         const uint32_t clock = timerClock(timerHardware->tim);
 221         /* used to find the desired timer frequency for max resolution */
 222         const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
 223         const uint32_t hz = clock / prescaler;
 224         const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff;
 225
 226         /*
 227             if brushed then it is the entire length of the period.
 228             TODO: this can be moved back to periodMin and periodLen
 229             once mixer outputs a 0..1 float value.
 230         */
 231         motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
 232         motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
 233
 234         pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion);
 235
 236         bool timerAlreadyUsed = false;
 237         for (int i = 0; i < motorIndex; i++) {
 238             if (motors[i].channel.tim == motors[motorIndex].channel.tim) {
 239                 timerAlreadyUsed = true;
 240                 break;
 241             }
 242         }
 243         motors[motorIndex].forceOverflow = !timerAlreadyUsed;
 244         motors[motorIndex].enabled = true;
 245     }
 246     return true;
 247 }
 248
 249 pwmOutputPort_t *pwmGetMotors(void)
 250 {
 251     return motors;
 252 }
 253
 254 #ifdef USE_SERVOS
 255 static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
 256
 257 void pwmWriteServo(uint8_t index, float value)
 258 {
 259     if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) {
 260         *servos[index].channel.ccr = lrintf(value);
 261     }
 262 }
 263
 264 void servoDevInit(const servoDevConfig_t *servoConfig)
 265 {
 266     for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
 267         const ioTag_t tag = servoConfig->ioTags[servoIndex];
 268
 269         if (!tag) {
 270             break;
 271         }
 272
 273         servos[servoIndex].io = IOGetByTag(tag);
 274
 275         IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
 276
 277         const timerHardware_t *timer = timerAllocate(tag, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
 278
 279         if (timer == NULL) {
 280             /* flag failure and disable ability to arm */
 281             break;
 282         }
 283
 284         IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
 285
 286         pwmOutConfig(&servos[servoIndex].channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
 287         servos[servoIndex].enabled = true;
 288     }
 289 }
 290 #endif // USE_SERVOS
 291 #endif // USE_PWM_OUTPUT