mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Refactoring motor to simplify implementation on other platforms (#14156)
This commit is contained in:
parent
7ed1b4b71f
commit
c2768d0409
62 changed files with 975 additions and 810 deletions
|
@ -36,12 +36,11 @@
|
|||
#include "drivers/dma_reqmap.h"
|
||||
#include "drivers/dshot.h"
|
||||
#include "drivers/dshot_bitbang.h"
|
||||
#include "drivers/dshot_bitbang_impl.h"
|
||||
#include "dshot_bitbang_impl.h"
|
||||
#include "drivers/dshot_command.h"
|
||||
#include "drivers/motor.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring
|
||||
#include "drivers/dshot_dpwm.h" // XXX for motorDmaOutput_t *getMotorDmaOutput(uint8_t index); should go away with refactoring
|
||||
#include "pwm_output_dshot_shared.h"
|
||||
#include "drivers/dshot_bitbang_decode.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/timer.h"
|
||||
|
@ -51,17 +50,6 @@
|
|||
// Maximum time to wait for telemetry reception to complete
|
||||
#define DSHOT_TELEMETRY_TIMEOUT 2000
|
||||
|
||||
FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS]; // TIM1 or TIM8
|
||||
FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
|
||||
|
||||
FAST_DATA_ZERO_INIT bbPort_t bbPorts[MAX_SUPPORTED_MOTOR_PORTS];
|
||||
FAST_DATA_ZERO_INIT int usedMotorPorts;
|
||||
|
||||
FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS];
|
||||
|
||||
static FAST_DATA_ZERO_INIT int motorCount;
|
||||
dshotBitbangStatus_e bbStatus;
|
||||
|
||||
// For MCUs that use MPU to control DMA coherency, there might be a performance hit
|
||||
// on manipulating input buffer content especially if it is read multiple times,
|
||||
// as the buffer region is attributed as not cachable.
|
||||
|
@ -91,10 +79,9 @@ const timerHardware_t bbTimerHardware[] = {
|
|||
DEF_TIM(TMR1, CH4, NONE, 0, 3, 0),
|
||||
};
|
||||
|
||||
static FAST_DATA_ZERO_INIT motorDevice_t bbDevice;
|
||||
static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
|
||||
|
||||
static motorPwmProtocolTypes_e motorPwmProtocol;
|
||||
static motorProtocolTypes_e motorProtocol;
|
||||
|
||||
// DMA GPIO output buffer formatting
|
||||
|
||||
|
@ -244,15 +231,15 @@ const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer)
|
|||
|
||||
// Return frequency of smallest change [state/sec]
|
||||
|
||||
static uint32_t getDshotBaseFrequency(motorPwmProtocolTypes_e pwmProtocolType)
|
||||
static uint32_t getDshotBaseFrequency(motorProtocolTypes_e pwmProtocolType)
|
||||
{
|
||||
switch (pwmProtocolType) {
|
||||
case(PWM_TYPE_DSHOT600):
|
||||
case(MOTOR_PROTOCOL_DSHOT600):
|
||||
return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
|
||||
case(PWM_TYPE_DSHOT300):
|
||||
case(MOTOR_PROTOCOL_DSHOT300):
|
||||
return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
|
||||
default:
|
||||
case(PWM_TYPE_DSHOT150):
|
||||
case(MOTOR_PROTOCOL_DSHOT150):
|
||||
return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
|
||||
}
|
||||
}
|
||||
|
@ -365,7 +352,7 @@ static void bbFindPacerTimer(void)
|
|||
}
|
||||
}
|
||||
|
||||
static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProtocolType)
|
||||
static void bbTimebaseSetup(bbPort_t *bbPort, motorProtocolTypes_e dshotProtocolType)
|
||||
{
|
||||
uint32_t timerclock = timerClock(bbPort->timhw->tim);
|
||||
|
||||
|
@ -383,7 +370,7 @@ static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProto
|
|||
// it does not use the timer channel associated with the pin.
|
||||
//
|
||||
|
||||
static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
|
||||
static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output)
|
||||
{
|
||||
int pinIndex = IO_GPIOPinIdx(io);
|
||||
int portIndex = IO_GPIOPortIdx(io);
|
||||
|
@ -410,10 +397,6 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
|
|||
}
|
||||
|
||||
if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
|
||||
bbDevice.vTable.write = motorWriteNull;
|
||||
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
|
||||
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -511,7 +494,7 @@ static bool bbDecodeTelemetry(void)
|
|||
SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
|
||||
}
|
||||
#endif
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
|
||||
|
||||
uint32_t rawValue = decode_bb_bitband(
|
||||
bbMotors[motorIndex].bbPort->portInputBuffer,
|
||||
|
@ -594,7 +577,7 @@ static void bbUpdateComplete(void)
|
|||
// If there is a dshot command loaded up, time it correctly with motor update
|
||||
|
||||
if (!dshotCommandQueueEmpty()) {
|
||||
if (!dshotCommandOutputIsEnabled(bbDevice.count)) {
|
||||
if (!dshotCommandOutputIsEnabled(dshotMotorCount)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
@ -641,7 +624,7 @@ static void bbUpdateComplete(void)
|
|||
|
||||
static bool bbEnableMotors(void)
|
||||
{
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
for (int i = 0; i < dshotMotorCount; i++) {
|
||||
if (bbMotors[i].configured) {
|
||||
IOConfigGPIO(bbMotors[i].io, bbMotors[i].iocfg);
|
||||
}
|
||||
|
@ -659,7 +642,7 @@ static void bbShutdown(void)
|
|||
return;
|
||||
}
|
||||
|
||||
static bool bbIsMotorEnabled(uint8_t index)
|
||||
static bool bbIsMotorEnabled(unsigned index)
|
||||
{
|
||||
return bbMotors[index].enabled;
|
||||
}
|
||||
|
@ -668,17 +651,13 @@ static void bbPostInit(void)
|
|||
{
|
||||
bbFindPacerTimer();
|
||||
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
|
||||
|
||||
if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) {
|
||||
if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) {
|
||||
return;
|
||||
}
|
||||
|
||||
bbMotors[motorIndex].enabled = true;
|
||||
|
||||
// Fill in motors structure for 4way access (XXX Should be refactored)
|
||||
|
||||
motors[motorIndex].enabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -696,6 +675,8 @@ static motorVTable_t bbVTable = {
|
|||
.convertExternalToMotor = dshotConvertFromExternal,
|
||||
.convertMotorToExternal = dshotConvertToExternal,
|
||||
.shutdown = bbShutdown,
|
||||
.isMotorIdle = bbDshotIsMotorIdle,
|
||||
.requestTelemetry = bbDshotRequestTelemetry,
|
||||
};
|
||||
|
||||
dshotBitbangStatus_e dshotBitbangGetStatus(void)
|
||||
|
@ -703,14 +684,15 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void)
|
|||
return bbStatus;
|
||||
}
|
||||
|
||||
motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count)
|
||||
bool dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)
|
||||
{
|
||||
dbgPinLo(0);
|
||||
dbgPinLo(1);
|
||||
|
||||
motorPwmProtocol = motorConfig->motorPwmProtocol;
|
||||
bbDevice.vTable = bbVTable;
|
||||
motorCount = count;
|
||||
motorProtocol = motorConfig->motorProtocol;
|
||||
device->vTable = &bbVTable;
|
||||
dshotMotorCount = device->count;
|
||||
|
||||
bbStatus = DSHOT_BITBANG_STATUS_OK;
|
||||
|
||||
#ifdef USE_DSHOT_TELEMETRY
|
||||
|
@ -719,12 +701,12 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
|
|||
|
||||
memset(bbOutputBuffer, 0, sizeof(bbOutputBuffer));
|
||||
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
|
||||
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
|
||||
const timerHardware_t *timerHardware = timerGetConfiguredByTag(motorConfig->ioTags[reorderedMotorIndex]);
|
||||
const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]);
|
||||
|
||||
uint8_t output = motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output;
|
||||
uint8_t output = motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output;
|
||||
bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP;
|
||||
|
||||
#ifdef USE_DSHOT_TELEMETRY
|
||||
|
@ -735,11 +717,10 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
|
|||
|
||||
if (!IOIsFreeOrPreinit(io)) {
|
||||
/* not enough motors initialised for the mixer or a break in the motors */
|
||||
bbDevice.vTable.write = motorWriteNull;
|
||||
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
|
||||
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
|
||||
bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
|
||||
return NULL;
|
||||
device->vTable = NULL;
|
||||
dshotMotorCount = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
int pinIndex = IO_GPIOPinIdx(io);
|
||||
|
@ -756,12 +737,8 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
|
|||
} else {
|
||||
IOHi(io);
|
||||
}
|
||||
|
||||
// Fill in motors structure for 4way access (TODO: Should be refactored)
|
||||
motors[motorIndex].io = bbMotors[motorIndex].io;
|
||||
}
|
||||
|
||||
return &bbDevice;
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif // USE_DSHOT_BB
|
||||
|
|
|
@ -36,11 +36,10 @@
|
|||
#include "drivers/dma.h"
|
||||
#include "drivers/dma_reqmap.h"
|
||||
#include "drivers/dshot.h"
|
||||
#include "drivers/dshot_bitbang_impl.h"
|
||||
#include "dshot_bitbang_impl.h"
|
||||
#include "drivers/dshot_command.h"
|
||||
#include "drivers/motor.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/timer.h"
|
||||
|
||||
|
|
|
@ -113,7 +113,9 @@ MCU_COMMON_SRC = \
|
|||
drivers/accgyro/accgyro_mpu.c \
|
||||
drivers/dshot_bitbang_decode.c \
|
||||
drivers/inverter.c \
|
||||
drivers/pwm_output_dshot_shared.c \
|
||||
common/stm32/pwm_output_dshot_shared.c \
|
||||
common/stm32/dshot_dpwm.c \
|
||||
common/stm32/dshot_bitbang_shared.c \
|
||||
$(MIDDLEWARES_DIR)/i2c_application_library/i2c_application.c \
|
||||
drivers/bus_i2c_timing.c \
|
||||
drivers/usb_msc_common.c \
|
||||
|
@ -133,6 +135,8 @@ MCU_COMMON_SRC = \
|
|||
msc/usbd_storage_sd_spi.c
|
||||
|
||||
SPEED_OPTIMISED_SRC += \
|
||||
common/stm32/dshot_bitbang_shared.c \
|
||||
common/stm32/pwm_output_dshot_shared.c \
|
||||
common/stm32/bus_spi_hw.c \
|
||||
common/stm32/system.c
|
||||
|
||||
|
|
|
@ -29,15 +29,13 @@
|
|||
#ifdef USE_PWM_OUTPUT
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/motor.h"
|
||||
#include "drivers/motor_impl.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/timer.h"
|
||||
|
||||
#include "pg/motor.h"
|
||||
|
||||
FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
|
||||
|
||||
static void pwmOCConfig(tmr_type *tim, uint8_t channel, uint16_t value, uint8_t output)
|
||||
{
|
||||
tmr_output_config_type tmr_OCInitStruct;
|
||||
|
@ -77,13 +75,7 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
|
|||
*channel->ccr = 0;
|
||||
}
|
||||
|
||||
static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice;
|
||||
|
||||
static void pwmWriteUnused(uint8_t index, float value)
|
||||
{
|
||||
UNUSED(index);
|
||||
UNUSED(value);
|
||||
}
|
||||
static FAST_DATA_ZERO_INIT motorDevice_t *pwmMotorDevice;
|
||||
|
||||
static void pwmWriteStandard(uint8_t index, float value)
|
||||
{
|
||||
|
@ -93,7 +85,7 @@ static void pwmWriteStandard(uint8_t index, float value)
|
|||
|
||||
void pwmShutdownPulsesForAllMotors(void)
|
||||
{
|
||||
for (int index = 0; index < motorPwmDevice.count; index++) {
|
||||
for (int index = 0; index < pwmMotorCount; index++) {
|
||||
// Set the compare register to 0, which stops the output pulsing if the timer overflows
|
||||
if (motors[index].channel.ccr) {
|
||||
*motors[index].channel.ccr = 0;
|
||||
|
@ -110,17 +102,23 @@ static motorVTable_t motorPwmVTable;
|
|||
bool pwmEnableMotors(void)
|
||||
{
|
||||
/* check motors can be enabled */
|
||||
return (motorPwmVTable.write != &pwmWriteUnused);
|
||||
return (pwmMotorDevice->vTable);
|
||||
}
|
||||
|
||||
bool pwmIsMotorEnabled(uint8_t index)
|
||||
bool pwmIsMotorEnabled(unsigned index)
|
||||
{
|
||||
return motors[index].enabled;
|
||||
}
|
||||
|
||||
static void pwmCompleteOneshotMotorUpdate(void)
|
||||
static bool useContinuousUpdate = true;
|
||||
|
||||
static void pwmCompleteMotorUpdate(void)
|
||||
{
|
||||
for (int index = 0; index < motorPwmDevice.count; index++) {
|
||||
if (useContinuousUpdate) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int index = 0; index < pwmMotorCount; index++) {
|
||||
if (motors[index].forceOverflow) {
|
||||
timerForceOverflow(motors[index].channel.tim);
|
||||
}
|
||||
|
@ -148,58 +146,66 @@ static motorVTable_t motorPwmVTable = {
|
|||
.shutdown = pwmShutdownPulsesForAllMotors,
|
||||
.convertExternalToMotor = pwmConvertFromExternal,
|
||||
.convertMotorToExternal = pwmConvertToExternal,
|
||||
.write = pwmWriteStandard,
|
||||
.decodeTelemetry = motorDecodeTelemetryNull,
|
||||
.updateComplete = pwmCompleteMotorUpdate,
|
||||
.requestTelemetry = NULL,
|
||||
.isMotorIdle = NULL,
|
||||
};
|
||||
|
||||
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
|
||||
bool motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)
|
||||
{
|
||||
memset(motors, 0, sizeof(motors));
|
||||
|
||||
motorPwmDevice.vTable = motorPwmVTable;
|
||||
if (!device) {
|
||||
return false;
|
||||
}
|
||||
|
||||
device->vTable = &motorPwmVTable;
|
||||
pwmMotorDevice = device;
|
||||
pwmMotorCount = device->count;
|
||||
useContinuousUpdate = motorConfig->useContinuousUpdate;
|
||||
|
||||
float sMin = 0;
|
||||
float sLen = 0;
|
||||
switch (motorConfig->motorPwmProtocol) {
|
||||
switch (motorConfig->motorProtocol) {
|
||||
default:
|
||||
case PWM_TYPE_ONESHOT125:
|
||||
case MOTOR_PROTOCOL_ONESHOT125:
|
||||
sMin = 125e-6f;
|
||||
sLen = 125e-6f;
|
||||
break;
|
||||
case PWM_TYPE_ONESHOT42:
|
||||
case MOTOR_PROTOCOL_ONESHOT42:
|
||||
sMin = 42e-6f;
|
||||
sLen = 42e-6f;
|
||||
break;
|
||||
case PWM_TYPE_MULTISHOT:
|
||||
case MOTOR_PROTOCOL_MULTISHOT:
|
||||
sMin = 5e-6f;
|
||||
sLen = 20e-6f;
|
||||
break;
|
||||
case PWM_TYPE_BRUSHED:
|
||||
case MOTOR_PROTOCOL_BRUSHED:
|
||||
sMin = 0;
|
||||
useUnsyncedPwm = true;
|
||||
useContinuousUpdate = true;
|
||||
idlePulse = 0;
|
||||
break;
|
||||
case PWM_TYPE_STANDARD:
|
||||
case MOTOR_PROTOCOL_PWM :
|
||||
sMin = 1e-3f;
|
||||
sLen = 1e-3f;
|
||||
useUnsyncedPwm = true;
|
||||
useContinuousUpdate = true;
|
||||
idlePulse = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
motorPwmDevice.vTable.write = pwmWriteStandard;
|
||||
motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
|
||||
motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
|
||||
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
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 */
|
||||
motorPwmDevice.vTable.write = &pwmWriteUnused;
|
||||
motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
|
||||
device->vTable = NULL;
|
||||
pwmMotorCount = 0;
|
||||
/* TODO: block arming and add reason system cannot arm */
|
||||
return NULL;
|
||||
return false;
|
||||
}
|
||||
|
||||
motors[motorIndex].io = IOGetByTag(tag);
|
||||
|
@ -209,23 +215,23 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
|
|||
|
||||
/* standard PWM outputs */
|
||||
// margin of safety is 4 periods when unsynced
|
||||
const unsigned pwmRateHz = useUnsyncedPwm ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
|
||||
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 = useUnsyncedPwm ? hz / pwmRateHz : 0xffff;
|
||||
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.
|
||||
*/
|
||||
motors[motorIndex].pulseScale = ((motorConfig->motorPwmProtocol == PWM_TYPE_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
|
||||
motors[motorIndex].pulseScale = ((motorConfig->motorProtocol == MOTOR_PROTOCOL_BRUSHED) ? period : (sLen * hz)) / 1000.0f;
|
||||
motors[motorIndex].pulseOffset = (sMin * hz) - (motors[motorIndex].pulseScale * 1000);
|
||||
|
||||
pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorPwmInversion);
|
||||
pwmOutConfig(&motors[motorIndex].channel, timerHardware, hz, period, idlePulse, motorConfig->motorInversion);
|
||||
|
||||
bool timerAlreadyUsed = false;
|
||||
for (int i = 0; i < motorIndex; i++) {
|
||||
|
@ -237,8 +243,7 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
|
|||
motors[motorIndex].forceOverflow = !timerAlreadyUsed;
|
||||
motors[motorIndex].enabled = true;
|
||||
}
|
||||
|
||||
return &motorPwmDevice;
|
||||
return true;
|
||||
}
|
||||
|
||||
pwmOutputPort_t *pwmGetMotors(void)
|
||||
|
|
|
@ -40,9 +40,9 @@
|
|||
|
||||
#include "drivers/pwm_output.h"
|
||||
#include "drivers/dshot.h"
|
||||
#include "drivers/dshot_dpwm.h"
|
||||
#include "dshot_dpwm.h"
|
||||
#include "drivers/dshot_command.h"
|
||||
#include "drivers/pwm_output_dshot_shared.h"
|
||||
#include "pwm_output_dshot_shared.h"
|
||||
|
||||
/**
|
||||
* Convert from BF channel to AT32 constants for timer output channels
|
||||
|
@ -115,9 +115,9 @@ tmr_channel_select_type toCHSelectType(const uint8_t bfChannel, const bool useNC
|
|||
* Called once for every dshot update if telemetry is being used (not just enabled by #def)
|
||||
* Called from pwm_output_dshot_shared.c pwmTelemetryDecode
|
||||
*/
|
||||
void dshotEnableChannels(uint8_t motorCount)
|
||||
void dshotEnableChannels(unsigned motorCount)
|
||||
{
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
for (unsigned i = 0; i < motorCount; i++) {
|
||||
tmr_primary_mode_select(dmaMotors[i].timerHardware->tim, TMR_PRIMARY_SEL_COMPARE);
|
||||
|
||||
tmr_channel_select_type atCh = toCHSelectType(dmaMotors[i].timerHardware->channel, dmaMotors[i].output & TIMER_OUTPUT_N_CHANNEL);
|
||||
|
@ -131,7 +131,7 @@ void dshotEnableChannels(uint8_t motorCount)
|
|||
* Set the timer and dma of the specified motor for use as an output
|
||||
*
|
||||
* Called from pwmDshotMotorHardwareConfig in this file and also from
|
||||
* pwmTelemetryDecode in src/main/drivers/pwm_output_dshot_shared.c
|
||||
* pwmTelemetryDecode in src/main/common/stm32/pwm_output_dshot_shared.c
|
||||
*/
|
||||
FAST_CODE void pwmDshotSetDirectionOutput(
|
||||
motorDmaOutput_t * const motor
|
||||
|
@ -250,7 +250,7 @@ void pwmCompleteDshotMotorUpdate(void)
|
|||
{
|
||||
// If there is a dshot command loaded up, time it correctly with motor update
|
||||
if (!dshotCommandQueueEmpty()) {
|
||||
if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
|
||||
if (!dshotCommandOutputIsEnabled(dshotMotorCount)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
@ -361,7 +361,7 @@ FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor)
|
|||
}
|
||||
|
||||
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex,
|
||||
motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
|
||||
motorProtocolTypes_e pwmProtocolType, uint8_t output)
|
||||
{
|
||||
#ifdef USE_DSHOT_TELEMETRY
|
||||
#define OCINIT motor->ocInitStruct
|
||||
|
@ -451,7 +451,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
|
|||
tmr_counter_enable(timer, FALSE);
|
||||
|
||||
uint32_t prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
|
||||
uint32_t period = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
|
||||
uint32_t period = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
|
||||
|
||||
tmr_clock_source_div_set(timer, TMR_CLOCK_DIV1);
|
||||
tmr_repetition_counter_set(timer, 0);
|
||||
|
@ -523,7 +523,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
|
|||
DMAINIT.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
|
||||
|
||||
DMAINIT.DMA_PeripheralBaseAddr = (uint32_t)&timerHardware->tim->DMAR;
|
||||
DMAINIT.DMA_BufferSize = (pwmProtocolType == PWM_TYPE_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX
|
||||
DMAINIT.DMA_BufferSize = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000) ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE; // XXX
|
||||
DMAINIT.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
|
||||
DMAINIT.DMA_MemoryInc = DMA_MemoryInc_Enable;
|
||||
DMAINIT.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
|
||||
|
@ -551,7 +551,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
|
|||
#ifdef USE_DSHOT_TELEMETRY
|
||||
motor->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 *
|
||||
(16 * MOTOR_BITLENGTH) / getDshotHz(pwmProtocolType);
|
||||
motor->timer->outputPeriod = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
|
||||
motor->timer->outputPeriod = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
|
||||
pwmDshotSetDirectionOutput(motor);
|
||||
#else
|
||||
pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue