mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 03:20:00 +03:00
281 lines
9.2 KiB
C
281 lines
9.2 KiB
C
/*
|
|
* This file is part of Betaflight.
|
|
*
|
|
* Betaflight is free software. You can redistribute this software
|
|
* and/or modify this software under the terms of the GNU General
|
|
* Public License as published by the Free Software Foundation,
|
|
* either version 3 of the License, or (at your option) any later
|
|
* version.
|
|
*
|
|
* Betaflight is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
|
*
|
|
* See the GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public
|
|
* License along with this software.
|
|
*
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
|
|
#include "platform.h"
|
|
|
|
#ifdef USE_PWM_OUTPUT
|
|
|
|
#include "drivers/io.h"
|
|
#include "drivers/motor_impl.h"
|
|
#include "drivers/pwm_output.h"
|
|
#include "drivers/pwm_output_impl.h"
|
|
#include "drivers/time.h"
|
|
#include "drivers/timer.h"
|
|
|
|
#include "pg/motor.h"
|
|
|
|
static void pwmOCConfig(tmr_type *tim, uint8_t channel, uint16_t value, uint8_t output)
|
|
{
|
|
tmr_output_config_type tmr_OCInitStruct;
|
|
tmr_output_default_para_init(&tmr_OCInitStruct);
|
|
tmr_OCInitStruct.oc_mode= TMR_OUTPUT_CONTROL_PWM_MODE_A;
|
|
|
|
if (output & TIMER_OUTPUT_N_CHANNEL) {
|
|
tmr_OCInitStruct.occ_output_state = TRUE;
|
|
tmr_OCInitStruct.occ_idle_state = FALSE;
|
|
tmr_OCInitStruct.occ_polarity = (output & TIMER_OUTPUT_INVERTED) ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH;
|
|
} else {
|
|
tmr_OCInitStruct.oc_output_state = TRUE;
|
|
tmr_OCInitStruct.oc_idle_state = TRUE;
|
|
tmr_OCInitStruct.oc_polarity = (output & TIMER_OUTPUT_INVERTED) ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH;
|
|
}
|
|
tmr_channel_value_set(tim, (channel-1)*2, value);
|
|
tmr_output_channel_config(tim,(channel-1)*2, &tmr_OCInitStruct);
|
|
tmr_output_channel_buffer_enable(tim, ((channel-1)*2),TRUE);
|
|
}
|
|
|
|
void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
|
|
{
|
|
configTimeBase(timerHardware->tim, period, hz);
|
|
pwmOCConfig(timerHardware->tim,
|
|
timerHardware->channel,
|
|
value,
|
|
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
|
|
);
|
|
|
|
tmr_output_enable(timerHardware->tim, TRUE);
|
|
tmr_counter_enable(timerHardware->tim, TRUE);
|
|
|
|
channel->ccr = timerChCCR(timerHardware);
|
|
|
|
channel->tim = timerHardware->tim;
|
|
|
|
*channel->ccr = 0;
|
|
}
|
|
|
|
static FAST_DATA_ZERO_INIT motorDevice_t *pwmMotorDevice;
|
|
|
|
static void pwmWriteStandard(uint8_t index, float value)
|
|
{
|
|
/* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
|
|
*pwmMotors[index].channel.ccr = lrintf((value * pwmMotors[index].pulseScale) + pwmMotors[index].pulseOffset);
|
|
}
|
|
|
|
static void pwmShutdownPulsesForAllMotors(void)
|
|
{
|
|
for (int index = 0; index < pwmMotorCount; index++) {
|
|
// Set the compare register to 0, which stops the output pulsing if the timer overflows
|
|
if (pwmMotors[index].channel.ccr) {
|
|
*pwmMotors[index].channel.ccr = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void pwmDisableMotors(void)
|
|
{
|
|
pwmShutdownPulsesForAllMotors();
|
|
}
|
|
|
|
static bool useContinuousUpdate = true;
|
|
|
|
static void pwmCompleteMotorUpdate(void)
|
|
{
|
|
if (useContinuousUpdate) {
|
|
return;
|
|
}
|
|
|
|
for (int index = 0; index < pwmMotorCount; index++) {
|
|
if (pwmMotors[index].forceOverflow) {
|
|
timerForceOverflow(pwmMotors[index].channel.tim);
|
|
}
|
|
// Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
|
|
// This compare register will be set to the output value on the next main loop.
|
|
*pwmMotors[index].channel.ccr = 0;
|
|
}
|
|
}
|
|
|
|
static float pwmConvertFromExternal(uint16_t externalValue)
|
|
{
|
|
return (float)externalValue;
|
|
}
|
|
|
|
static uint16_t pwmConvertToExternal(float motorValue)
|
|
{
|
|
return (uint16_t)motorValue;
|
|
}
|
|
|
|
static motorVTable_t motorPwmVTable = {
|
|
.postInit = motorPostInitNull,
|
|
.enable = pwmEnableMotors,
|
|
.disable = pwmDisableMotors,
|
|
.isMotorEnabled = pwmIsMotorEnabled,
|
|
.shutdown = pwmShutdownPulsesForAllMotors,
|
|
.convertExternalToMotor = pwmConvertFromExternal,
|
|
.convertMotorToExternal = pwmConvertToExternal,
|
|
.write = pwmWriteStandard,
|
|
.decodeTelemetry = motorDecodeTelemetryNull,
|
|
.updateComplete = pwmCompleteMotorUpdate,
|
|
.requestTelemetry = NULL,
|
|
.isMotorIdle = NULL,
|
|
.getMotorIO = pwmGetMotorIO,
|
|
};
|
|
|
|
bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)
|
|
{
|
|
memset(pwmMotors, 0, sizeof(pwmMotors));
|
|
|
|
if (!device) {
|
|
return false;
|
|
}
|
|
|
|
device->vTable = &motorPwmVTable;
|
|
pwmMotorDevice = device;
|
|
pwmMotorCount = device->count;
|
|
useContinuousUpdate = motorConfig->useContinuousUpdate;
|
|
|
|
float sMin = 0;
|
|
float sLen = 0;
|
|
switch (motorConfig->motorProtocol) {
|
|
default:
|
|
case MOTOR_PROTOCOL_ONESHOT125:
|
|
sMin = 125e-6f;
|
|
sLen = 125e-6f;
|
|
break;
|
|
case MOTOR_PROTOCOL_ONESHOT42:
|
|
sMin = 42e-6f;
|
|
sLen = 42e-6f;
|
|
break;
|
|
case MOTOR_PROTOCOL_MULTISHOT:
|
|
sMin = 5e-6f;
|
|
sLen = 20e-6f;
|
|
break;
|
|
case MOTOR_PROTOCOL_BRUSHED:
|
|
sMin = 0;
|
|
useContinuousUpdate = true;
|
|
idlePulse = 0;
|
|
break;
|
|
case MOTOR_PROTOCOL_PWM :
|
|
sMin = 1e-3f;
|
|
sLen = 1e-3f;
|
|
useContinuousUpdate = true;
|
|
idlePulse = 0;
|
|
break;
|
|
}
|
|
|
|
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < pwmMotorCount; motorIndex++) {
|
|
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
|
|
const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
|
|
const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
|
|
|
|
if (timerHardware == NULL) {
|
|
/* not enough motors initialised for the mixer or a break in the motors */
|
|
device->vTable = NULL;
|
|
pwmMotorCount = 0;
|
|
/* TODO: block arming and add reason system cannot arm */
|
|
return false;
|
|
}
|
|
|
|
pwmMotors[motorIndex].io = IOGetByTag(tag);
|
|
IOInit(pwmMotors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
|
|
|
|
IOConfigGPIOAF(pwmMotors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
|
|
|
|
/* standard PWM outputs */
|
|
// margin of safety is 4 periods when unsynced
|
|
const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
|
|
|
|
const uint32_t clock = timerClock(timerHardware->tim);
|
|
/* used to find the desired timer frequency for max resolution */
|
|
const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
|
|
const uint32_t hz = clock / prescaler;
|
|
const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff;
|
|
|
|
/*
|
|
if brushed then it is the entire length of the period.
|
|
TODO: this can be moved back to periodMin and periodLen
|
|
once mixer outputs a 0..1 float value.
|
|
*/
|
|
pwmMotors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
|
|
pwmMotors[motorIndex].pulseOffset = (sMin * hz) - (pwmMotors[motorIndex].pulseScale * 1000);
|
|
|
|
pwmOutConfig(&pwmMotors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion);
|
|
|
|
bool timerAlreadyUsed = false;
|
|
for (int i = 0; i < motorIndex; i++) {
|
|
if (pwmMotors[i].channel.tim == pwmMotors[motorIndex].channel.tim) {
|
|
timerAlreadyUsed = true;
|
|
break;
|
|
}
|
|
}
|
|
pwmMotors[motorIndex].forceOverflow = !timerAlreadyUsed;
|
|
pwmMotors[motorIndex].enabled = true;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
pwmOutputPort_t *pwmGetMotors(void)
|
|
{
|
|
return pwmMotors;
|
|
}
|
|
|
|
#ifdef USE_SERVOS
|
|
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
|
|
|
|
void pwmWriteServo(uint8_t index, float value)
|
|
{
|
|
if (index < MAX_SUPPORTED_SERVOS && servos[index].channel.ccr) {
|
|
*servos[index].channel.ccr = lrintf(value);
|
|
}
|
|
}
|
|
|
|
void servoDevInit(const servoDevConfig_t *servoConfig)
|
|
{
|
|
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
|
const ioTag_t tag = servoConfig->ioTags[servoIndex];
|
|
|
|
if (!tag) {
|
|
break;
|
|
}
|
|
|
|
servos[servoIndex].io = IOGetByTag(tag);
|
|
|
|
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
|
|
|
|
const timerHardware_t *timer = timerAllocate(tag, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
|
|
|
|
if (timer == NULL) {
|
|
/* flag failure and disable ability to arm */
|
|
break;
|
|
}
|
|
|
|
IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
|
|
|
|
pwmOutConfig(&servos[servoIndex].channel, timer, PWM_TIMER_1MHZ, PWM_TIMER_1MHZ / servoConfig->servoPwmRate, servoConfig->servoCenterPulse, 0);
|
|
servos[servoIndex].enabled = true;
|
|
}
|
|
}
|
|
#endif // USE_SERVOS
|
|
#endif // USE_PWM_OUTPUT
|