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