1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00

Refactoring motors to simplify implementation on other platforms

This commit is contained in:
blckmn 2025-01-14 23:15:48 +11:00
parent c513b102b6
commit d3c113b4c5
56 changed files with 428 additions and 321 deletions

View file

@ -1719,8 +1719,8 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, "%d", armingConfig()->gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle", "%d", rxConfig()->airModeActivateThreshold);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_SERIAL_RX_PROVIDER, "%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_UNSYNCED_PWM, "%d", motorConfig()->dev.useUnsyncedPwm);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_PROTOCOL, "%d", motorConfig()->dev.motorPwmProtocol);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_USE_UNSYNCED_PWM, "%d", motorConfig()->dev.useUnsyncedUpdate);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_PROTOCOL, "%d", motorConfig()->dev.motorProtocol);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_PWM_RATE, "%d", motorConfig()->dev.motorPwmRate);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_IDLE, "%d", motorConfig()->motorIdle);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DEBUG_MODE, "%d", debugMode);

View file

@ -68,8 +68,6 @@ bool cliMode = false;
#include "drivers/dma_reqmap.h"
#include "drivers/dshot.h"
#include "drivers/dshot_command.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/pwm_output_dshot_shared.h"
#include "drivers/camera_control_impl.h"
#include "drivers/compass/compass.h"
#include "drivers/display.h"

View file

@ -934,10 +934,10 @@ const clivalue_t valueTable[] = {
{ "dshot_bitbang_timer", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DSHOT_BITBANGED_TIMER }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useDshotBitbangedTimer) },
#endif
#endif // USE_DSHOT
{ PARAM_NAME_USE_UNSYNCED_PWM, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useUnsyncedPwm) },
{ PARAM_NAME_MOTOR_PWM_PROTOCOL, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmProtocol) },
{ PARAM_NAME_USE_UNSYNCED_PWM, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.useUnsyncedUpdate) },
{ PARAM_NAME_MOTOR_PWM_PROTOCOL, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorProtocol) },
{ PARAM_NAME_MOTOR_PWM_RATE, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 200, 32000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmRate) },
{ "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorPwmInversion) },
{ "motor_pwm_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorInversion) },
{ PARAM_NAME_MOTOR_POLES, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 4, UINT8_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPoleCount) },
{ "motor_output_reordering", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = MAX_SUPPORTED_MOTORS, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.motorOutputReordering)},

View file

@ -42,8 +42,6 @@
#include "config/feature.h"
#include "config/simplified_tuning.h"
#include "drivers/pwm_output.h"
#include "config/config.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"

View file

@ -35,7 +35,7 @@
#include "common/printf.h"
#include "config/config.h"
#include "drivers/pwm_output.h"
#include "drivers/motor_types.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"

View file

@ -36,6 +36,7 @@
#include "config/config_eeprom.h"
#include "config/feature.h"
#include "drivers/dshot.h"
#include "drivers/dshot_command.h"
#include "drivers/motor.h"
#include "drivers/system.h"
@ -92,8 +93,6 @@
#include "config.h"
#include "drivers/dshot.h"
static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */
static bool rebootRequired = false; // set if a config change requires a reboot to take effect
@ -277,7 +276,7 @@ static void validateAndFixConfig(void)
#endif
}
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_BRUSHED) {
featureDisableImmediate(FEATURE_3D);
if (motorConfig()->mincommand < 1000) {
@ -285,7 +284,7 @@ static void validateAndFixConfig(void)
}
}
if ((motorConfig()->dev.motorPwmProtocol == PWM_TYPE_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
if ((motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_STANDARD) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
motorConfigMutable()->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
}
@ -381,7 +380,7 @@ static void validateAndFixConfig(void)
}
#if defined(USE_DSHOT_TELEMETRY) && defined(USE_DSHOT_BITBANG)
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_PROSHOT1000 && motorConfig()->dev.useDshotTelemetry &&
if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_PROSHOT1000 && motorConfig()->dev.useDshotTelemetry &&
motorConfig()->dev.useDshotBitbang == DSHOT_BITBANG_ON) {
motorConfigMutable()->dev.useDshotBitbang = DSHOT_BITBANG_AUTO;
}
@ -450,7 +449,7 @@ static void validateAndFixConfig(void)
#if defined(USE_DSHOT)
// If using DSHOT protocol disable unsynched PWM as it's meaningless
if (configuredMotorProtocolDshot) {
motorConfigMutable()->dev.useUnsyncedPwm = false;
motorConfigMutable()->dev.useUnsyncedUpdate = false;
}
#if defined(USE_DSHOT_TELEMETRY)
@ -590,29 +589,29 @@ void validateAndFixGyroConfig(void)
#endif
&& motorConfig()->dev.useDshotTelemetry
) {
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_DSHOT600) {
motorConfigMutable()->dev.motorPwmProtocol = PWM_TYPE_DSHOT300;
if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_DSHOT600) {
motorConfigMutable()->dev.motorProtocol = MOTOR_PROTOCOL_DSHOT300;
}
if (gyro.sampleRateHz > 4000) {
pidConfigMutable()->pid_process_denom = MAX(2, pidConfig()->pid_process_denom);
}
}
#endif // USE_DSHOT && USE_PID_DENOM_CHECK
switch (motorConfig()->dev.motorPwmProtocol) {
case PWM_TYPE_STANDARD:
switch (motorConfig()->dev.motorProtocol) {
case MOTOR_PROTOCOL_STANDARD:
motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE;
break;
case PWM_TYPE_ONESHOT125:
case MOTOR_PROTOCOL_ONESHOT125:
motorUpdateRestriction = 0.0005f;
break;
case PWM_TYPE_ONESHOT42:
case MOTOR_PROTOCOL_ONESHOT42:
motorUpdateRestriction = 0.0001f;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT150:
case MOTOR_PROTOCOL_DSHOT150:
motorUpdateRestriction = 0.000250f;
break;
case PWM_TYPE_DSHOT300:
case MOTOR_PROTOCOL_DSHOT300:
motorUpdateRestriction = 0.0001f;
break;
#endif
@ -621,11 +620,11 @@ void validateAndFixGyroConfig(void)
break;
}
if (motorConfig()->dev.useUnsyncedPwm) {
if (motorConfig()->dev.useUnsyncedUpdate) {
bool configuredMotorProtocolDshot = false;
checkMotorProtocolEnabled(&motorConfig()->dev, &configuredMotorProtocolDshot);
// Prevent overriding the max rate of motors
if (!configuredMotorProtocolDshot && motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD) {
if (!configuredMotorProtocolDshot && motorConfig()->dev.motorProtocol != MOTOR_PROTOCOL_STANDARD) {
const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
}

View file

@ -42,7 +42,6 @@
#include "drivers/io.h"
#include "drivers/sensor.h"
#include "drivers/time.h"
#include "drivers/pwm_output.h"
#include "sensors/gyro.h"
#include "pg/gyrodev.h"

View file

@ -26,6 +26,7 @@
#include "common/time.h"
#include "pg/motor.h"
#include "drivers/motor_types.h"
#define DSHOT_MIN_THROTTLE (48)
#define DSHOT_MAX_THROTTLE (2047)
@ -109,6 +110,23 @@ typedef struct dshotTelemetryState_s {
dshotRawValueState_t rawValueState;
} dshotTelemetryState_t;
#ifdef USE_DSHOT_TELEMETRY
extern uint32_t readDoneCount;
FAST_DATA_ZERO_INIT extern uint32_t inputStampUs;
typedef struct dshotTelemetryCycleCounters_s {
uint32_t irqAt;
uint32_t changeDirectionCompletedAt;
} dshotTelemetryCycleCounters_t;
FAST_DATA_ZERO_INIT extern dshotTelemetryCycleCounters_t dshotDMAHandlerCycleCounters;
#endif
struct motorDevConfig_s;
motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate);
extern dshotTelemetryState_t dshotTelemetryState;
#ifdef USE_DSHOT_TELEMETRY_STATS

View file

@ -30,13 +30,9 @@
#include "drivers/io.h"
#include "drivers/motor.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/pwm_output.h"
#include "dshot_command.h"
#include "drivers/dshot_command.h"
#define DSHOT_PROTOCOL_DETECTION_DELAY_MS 3000
#define DSHOT_INITIAL_DELAY_US 10000
@ -143,8 +139,7 @@ static dshotCommandControl_t* addCommand(void)
static bool allMotorsAreIdle(void)
{
for (unsigned i = 0; i < motorDeviceCount(); i++) {
const motorDmaOutput_t *motor = getMotorDmaOutput(i);
if (motor->protocolControl.value) {
if (motorIsMotorIdle(i)) {
return false;
}
}
@ -232,8 +227,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
}
for (uint8_t i = 0; i < motorDeviceCount(); i++) {
motorDmaOutput_t *const motor = getMotorDmaOutput(i);
motor->protocolControl.requestTelemetry = true;
vTable->requestTelemetry(i);
vTable->writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP);
}

View file

@ -60,11 +60,7 @@
#define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof(uint32_t))
DMA_RW_AXI __attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH];
#else
#if defined(STM32F7)
FAST_DATA_ZERO_INIT uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
#else
uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];
#endif
#endif
static ioTag_t ledStripIoTag;

View file

@ -32,10 +32,10 @@
#include "config/feature.h"
#include "drivers/dshot.h" // for DSHOT_ constants in initEscEndpoints; may be gone in the future
#include "drivers/dshot_bitbang.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/pwm_output.h" // for PWM_TYPE_* and others
#include "drivers/dshot.h"
#include "drivers/dshot_bitbang.h" // TODO: bitbang should be behind the veil of dshot (it is an implementation)
#include "drivers/pwm_output.h"
#include "drivers/time.h"
#include "fc/rc_controls.h" // for flight3DConfig_t
@ -57,11 +57,11 @@ void motorShutdown(void)
motorDevice->motorEnableTimeMs = 0;
motorDevice->initialized = false;
switch (motorConfig()->dev.motorPwmProtocol) {
case PWM_TYPE_STANDARD:
case PWM_TYPE_ONESHOT125:
case PWM_TYPE_ONESHOT42:
case PWM_TYPE_MULTISHOT:
switch (motorConfig()->dev.motorProtocol) {
case MOTOR_PROTOCOL_STANDARD:
case MOTOR_PROTOCOL_ONESHOT125:
case MOTOR_PROTOCOL_ONESHOT42:
case MOTOR_PROTOCOL_MULTISHOT:
// Delay 500ms will disarm esc which can prevent motor spin while reboot
shutdownDelayUs += 500 * 1000;
break;
@ -125,6 +125,17 @@ void motorWriteAll(float *values)
#endif
}
void motorRequestTelemetry(uint8_t index)
{
if (index > motorDevice->count - 1) {
return;
}
if (motorDevice->vTable.requestTelemetry) {
motorDevice->vTable.requestTelemetry(index);
}
}
unsigned motorDeviceCount(void)
{
return motorDevice->count;
@ -158,20 +169,20 @@ bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isP
bool enabled = false;
bool isDshot = false;
switch (motorDevConfig->motorPwmProtocol) {
case PWM_TYPE_STANDARD:
case PWM_TYPE_ONESHOT125:
case PWM_TYPE_ONESHOT42:
case PWM_TYPE_MULTISHOT:
case PWM_TYPE_BRUSHED:
switch (motorDevConfig->motorProtocol) {
case MOTOR_PROTOCOL_STANDARD:
case MOTOR_PROTOCOL_ONESHOT125:
case MOTOR_PROTOCOL_ONESHOT42:
case MOTOR_PROTOCOL_MULTISHOT:
case MOTOR_PROTOCOL_BRUSHED:
enabled = true;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT150:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_PROSHOT1000:
case MOTOR_PROTOCOL_DSHOT150:
case MOTOR_PROTOCOL_DSHOT300:
case MOTOR_PROTOCOL_DSHOT600:
case MOTOR_PROTOCOL_PROSHOT1000:
enabled = true;
isDshot = true;
break;
@ -240,7 +251,12 @@ static void motorDisableNull(void)
static bool motorIsEnabledNull(uint8_t index)
{
UNUSED(index);
return false;
}
static bool motorIsIdleNull(uint8_t index)
{
UNUSED(index);
return false;
}
@ -293,6 +309,7 @@ static const motorVTable_t motorNullVTable = {
.convertExternalToMotor = motorConvertFromExternalNull,
.convertMotorToExternal = motorConvertToExternalNull,
.shutdown = motorShutdownNull,
.isMotorIdle = motorIsIdleNull,
};
static motorDevice_t motorNullDevice = {
@ -318,7 +335,7 @@ bool isMotorProtocolBidirDshot(void)
void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount)
{
#if defined(USE_PWM_OUTPUT) || defined(USE_DSHOT)
bool useUnsyncedPwm = motorDevConfig->useUnsyncedPwm;
bool useUnsyncedUpdate = motorDevConfig->useUnsyncedUpdate;
#else
UNUSED(idlePulse);
UNUSED(motorDevConfig);
@ -328,7 +345,7 @@ void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, ui
do {
if (!isMotorProtocolDshot()) {
#ifdef USE_PWM_OUTPUT
motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm);
motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedUpdate);
#endif
break;
}
@ -339,7 +356,7 @@ void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, ui
break;
}
#endif
motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm);
motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedUpdate);
#endif
} while(0);
}
@ -390,6 +407,11 @@ bool motorIsMotorEnabled(uint8_t index)
return motorDevice->vTable.isMotorEnabled(index);
}
bool motorIsMotorIdle(uint8_t index)
{
return motorDevice->vTable.isMotorIdle ? motorDevice->vTable.isMotorIdle(index) : false;
}
#ifdef USE_DSHOT
timeMs_t motorGetMotorEnableTimeMs(void)
{
@ -402,10 +424,10 @@ bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig)
{
#if defined(STM32F4) || defined(APM32F4)
return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
(motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
(motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000);
#else
return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
(motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
(motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000);
#endif
}
#endif

View file

@ -26,48 +26,7 @@
#include "pg/motor.h"
typedef enum {
PWM_TYPE_STANDARD = 0,
PWM_TYPE_ONESHOT125,
PWM_TYPE_ONESHOT42,
PWM_TYPE_MULTISHOT,
PWM_TYPE_BRUSHED,
PWM_TYPE_DSHOT150,
PWM_TYPE_DSHOT300,
PWM_TYPE_DSHOT600,
// PWM_TYPE_DSHOT1200, removed
PWM_TYPE_PROSHOT1000,
PWM_TYPE_DISABLED,
PWM_TYPE_MAX
} motorPwmProtocolTypes_e;
typedef struct motorVTable_s {
// Common
void (*postInit)(void);
float (*convertExternalToMotor)(uint16_t externalValue);
uint16_t (*convertMotorToExternal)(float motorValue);
bool (*enable)(void);
void (*disable)(void);
bool (*isMotorEnabled)(uint8_t index);
bool (*telemetryWait)(void);
bool (*decodeTelemetry)(void);
void (*updateInit)(void);
void (*write)(uint8_t index, float value);
void (*writeInt)(uint8_t index, uint16_t value);
void (*updateComplete)(void);
void (*shutdown)(void);
// Digital commands
} motorVTable_t;
typedef struct motorDevice_s {
motorVTable_t vTable;
uint8_t count;
bool initialized;
bool enabled;
timeMs_t motorEnableTimeMs;
} motorDevice_t;
#include "drivers/motor_types.h"
void motorPostInitNull();
void motorWriteNull(uint8_t index, float value);
@ -76,6 +35,7 @@ void motorUpdateCompleteNull(void);
void motorPostInit();
void motorWriteAll(float *values);
void motorRequestTelemetry(uint8_t index);
void motorInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3DHigh, float *deadbandMotor3DLow);
@ -96,6 +56,7 @@ void motorEnable(void);
float motorEstimateMaxRpm(void);
bool motorIsEnabled(void);
bool motorIsMotorEnabled(uint8_t index);
bool motorIsMotorIdle(uint8_t index);
timeMs_t motorGetMotorEnableTimeMs(void);
void motorShutdown(void); // Replaces stopPwmAllMotors

View file

@ -0,0 +1,72 @@
/*
* 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/>.
*/
#pragma once
#include "common/time.h"
#define ALL_MOTORS 255
#define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1
#define MOTOR_OUTPUT_LIMIT_PERCENT_MAX 100
typedef enum {
MOTOR_PROTOCOL_STANDARD = 0,
MOTOR_PROTOCOL_ONESHOT125,
MOTOR_PROTOCOL_ONESHOT42,
MOTOR_PROTOCOL_MULTISHOT,
MOTOR_PROTOCOL_BRUSHED,
MOTOR_PROTOCOL_DSHOT150,
MOTOR_PROTOCOL_DSHOT300,
MOTOR_PROTOCOL_DSHOT600,
/* MOTOR_PROTOCOL_DSHOT1200, removed */
MOTOR_PROTOCOL_PROSHOT1000,
MOTOR_PROTOCOL_DISABLED,
MOTOR_PROTOCOL_MAX
} motorProtocolTypes_e;
typedef struct motorVTable_s {
// Common
void (*postInit)(void);
float (*convertExternalToMotor)(uint16_t externalValue);
uint16_t (*convertMotorToExternal)(float motorValue);
bool (*enable)(void);
void (*disable)(void);
bool (*isMotorEnabled)(uint8_t index);
bool (*telemetryWait)(void);
bool (*decodeTelemetry)(void);
void (*updateInit)(void);
void (*write)(uint8_t index, float value);
void (*writeInt)(uint8_t index, uint16_t value);
void (*updateComplete)(void);
void (*shutdown)(void);
bool (*isMotorIdle)(uint8_t index);
// Digital commands
void (*requestTelemetry)(uint8_t index);
} motorVTable_t;
typedef struct motorDevice_s {
motorVTable_t vTable;
uint8_t count;
bool initialized;
bool enabled;
timeMs_t motorEnableTimeMs;
} motorDevice_t;

View file

@ -29,16 +29,9 @@
#include "drivers/motor.h"
#include "drivers/timer.h"
#define BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 480
#define ALL_MOTORS 255
#define MOTOR_OUTPUT_LIMIT_PERCENT_MIN 1
#define MOTOR_OUTPUT_LIMIT_PERCENT_MAX 100
#define PWM_TIMER_1MHZ MHZ_TO_HZ(1)
// TODO: move the implementation defintions to impl header (platform)
struct timerHardware_s;
typedef struct {

View file

@ -181,11 +181,7 @@ static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg)
}
IOInit(IOGetByTag(tag), OWNER_MOTOR, 0);
#if defined(STM32F7) || defined(STM32H7)
IOConfigGPIOAF(IOGetByTag(tag), cfg, timhw->alternateFunction);
#else
IOConfigGPIO(IOGetByTag(tag), cfg);
#endif
}
static void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)

View file

@ -20,8 +20,6 @@
#pragma once
#include "drivers/pwm_output.h"
#define ESCSERIAL_BUFFER_SIZE 1024
typedef enum {

View file

@ -64,7 +64,6 @@
#include "drivers/nvic.h"
#include "drivers/persistent.h"
#include "drivers/pin_pull_up_down.h"
#include "drivers/pwm_output.h"
#include "drivers/rx/rx_pwm.h"
#include "drivers/sensor.h"
#include "drivers/serial.h"
@ -545,7 +544,7 @@ void init(void)
if (featureIsEnabled(FEATURE_3D)) {
idlePulse = flight3DConfig()->neutral3d;
}
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_BRUSHED) {
idlePulse = 0; // brushed motors
}
#ifdef USE_MOTOR

View file

@ -30,7 +30,7 @@
#include "pg/pg.h"
#include "drivers/io_types.h"
#include "drivers/pwm_output.h"
#include "drivers/motor.h"
#define QUAD_MOTOR_COUNT 4

View file

@ -36,7 +36,6 @@
#include "config/config_reset.h"
#include "config/simplified_tuning.h"
#include "drivers/pwm_output.h"
#include "drivers/sound_beeper.h"
#include "drivers/time.h"

View file

@ -36,7 +36,7 @@
#include "drivers/time.h"
#include "drivers/timer.h"
#include "drivers/light_led.h"
#include "drivers/pwm_output.h"
#include "flight/mixer.h"
#include "io/beeper.h"

View file

@ -1879,12 +1879,12 @@ case MSP_NAME:
case MSP_ADVANCED_CONFIG:
sbufWriteU8(dst, 1); // was gyroConfig()->gyro_sync_denom - removed in API 1.43
sbufWriteU8(dst, pidConfig()->pid_process_denom);
sbufWriteU8(dst, motorConfig()->dev.useUnsyncedPwm);
sbufWriteU8(dst, motorConfig()->dev.motorPwmProtocol);
sbufWriteU8(dst, motorConfig()->dev.useUnsyncedUpdate);
sbufWriteU8(dst, motorConfig()->dev.motorProtocol);
sbufWriteU16(dst, motorConfig()->dev.motorPwmRate);
sbufWriteU16(dst, motorConfig()->motorIdle);
sbufWriteU8(dst, 0); // DEPRECATED: gyro_use_32kHz
sbufWriteU8(dst, motorConfig()->dev.motorPwmInversion);
sbufWriteU8(dst, motorConfig()->dev.motorInversion);
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold);
@ -3087,8 +3087,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
case MSP_SET_ADVANCED_CONFIG:
sbufReadU8(src); // was gyroConfigMutable()->gyro_sync_denom - removed in API 1.43
pidConfigMutable()->pid_process_denom = sbufReadU8(src);
motorConfigMutable()->dev.useUnsyncedPwm = sbufReadU8(src);
motorConfigMutable()->dev.motorPwmProtocol = sbufReadU8(src);
motorConfigMutable()->dev.useUnsyncedUpdate = sbufReadU8(src);
motorConfigMutable()->dev.motorProtocol = sbufReadU8(src);
motorConfigMutable()->dev.motorPwmRate = sbufReadU16(src);
if (sbufBytesRemaining(src) >= 2) {
motorConfigMutable()->motorIdle = sbufReadU16(src);
@ -3097,7 +3097,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
sbufReadU8(src); // DEPRECATED: gyro_use_32khz
}
if (sbufBytesRemaining(src)) {
motorConfigMutable()->dev.motorPwmInversion = sbufReadU8(src);
motorConfigMutable()->dev.motorInversion = sbufReadU8(src);
}
if (sbufBytesRemaining(src) >= 8) {
gyroConfigMutable()->gyro_to_use = sbufReadU8(src);

View file

@ -25,12 +25,20 @@
#ifdef USE_MOTOR
#include "drivers/pwm_output.h"
#include "drivers/motor_types.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/motor.h"
#if !defined(BRUSHED_MOTORS_PWM_RATE)
#define BRUSHED_MOTORS_PWM_RATE 16000
#endif
#if !defined(BRUSHLESS_MOTORS_PWM_RATE)
#define BRUSHLESS_MOTORS_PWM_RATE 480
#endif
#if !defined(DEFAULT_DSHOT_BITBANG)
#define DEFAULT_DSHOT_BITBANG DSHOT_BITBANG_AUTO
#endif
@ -53,19 +61,19 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
{
#ifdef BRUSHED_MOTORS
motorConfig->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
motorConfig->dev.motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->dev.useUnsyncedPwm = true;
motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_BRUSHED;
motorConfig->dev.useUnsyncedUpdate = true;
#else
motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
#ifndef USE_DSHOT
if (motorConfig->dev.motorPwmProtocol == PWM_TYPE_STANDARD) {
motorConfig->dev.useUnsyncedPwm = true;
if (motorConfig->dev.motorProtocol == MOTOR_PROTOCOL_STANDARD) {
motorConfig->dev.useUnsyncedUpdate = true;
}
motorConfig->dev.motorPwmProtocol = PWM_TYPE_DISABLED;
motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_DISABLED;
#elif defined(DEFAULT_MOTOR_DSHOT_SPEED)
motorConfig->dev.motorPwmProtocol = DEFAULT_MOTOR_DSHOT_SPEED;
motorConfig->dev.motorProtocol = DEFAULT_MOTOR_DSHOT_SPEED;
#else
motorConfig->dev.motorPwmProtocol = PWM_TYPE_DSHOT600;
motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_DSHOT600;
#endif // USE_DSHOT
#endif // BRUSHED_MOTORS

View file

@ -25,6 +25,14 @@
#include "drivers/io.h"
#include "drivers/dshot_bitbang.h"
#if !defined(BRUSHED_MOTORS_PWM_RATE)
#define BRUSHED_MOTORS_PWM_RATE 16000
#endif
#if !defined(BRUSHLESS_MOTORS_PWM_RATE)
#define BRUSHLESS_MOTORS_PWM_RATE 480
#endif
typedef enum {
DSHOT_BITBANGED_TIMER_AUTO = 0,
DSHOT_BITBANGED_TIMER_TIM1,
@ -44,9 +52,9 @@ typedef enum {
typedef struct motorDevConfig_s {
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
uint8_t motorPwmProtocol; // Pwm Protocol
uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation
uint8_t useUnsyncedPwm;
uint8_t motorProtocol; // Pwm Protocol
uint8_t motorInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation
uint8_t useUnsyncedUpdate;
uint8_t useBurstDshot;
uint8_t useDshotTelemetry;
uint8_t useDshotEdt;

View file

@ -42,7 +42,6 @@
#include "drivers/timer.h"
#include "drivers/motor.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
@ -321,8 +320,7 @@ void escSensorProcess(timeUs_t currentTimeUs)
escTriggerTimestamp = currentTimeMs;
startEscDataRead(telemetryBuffer, TELEMETRY_FRAME_SIZE);
motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor);
motor->protocolControl.requestTelemetry = true;
motorRequestTelemetry(escSensorMotor);
escSensorTriggerState = ESC_SENSOR_TRIGGER_PENDING;
DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1);

View file

@ -35,15 +35,16 @@
#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 "dshot_dpwm.h" // XXX for motorDmaOutput_t *getMotorDmaOutput(uint8_t index); should go away with refactoring
#include "drivers/dshot_bitbang_decode.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "pwm_output_dshot_shared.h"
#include "pg/motor.h"
#include "pg/pinio.h"
@ -108,7 +109,7 @@ const timerHardware_t bbTimerHardware[] = {
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
@ -258,15 +259,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;
}
}
@ -379,7 +380,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);
@ -397,7 +398,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)
{
// Return if no GPIO is specified
if (!io) {
@ -677,7 +678,7 @@ static void bbPostInit(void)
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) {
if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) {
return;
}
@ -703,6 +704,8 @@ static motorVTable_t bbVTable = {
.convertExternalToMotor = dshotConvertFromExternal,
.convertMotorToExternal = dshotConvertToExternal,
.shutdown = bbShutdown,
.isMotorIdle = bbDshotIsMotorIdle,
.requestTelemetry = bbDshotRequestTelemetry,
};
dshotBitbangStatus_e dshotBitbangGetStatus(void)
@ -715,7 +718,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
dbgPinLo(0);
dbgPinLo(1);
motorPwmProtocol = motorConfig->motorPwmProtocol;
motorProtocol = motorConfig->motorProtocol;
bbDevice.vTable = bbVTable;
motorCount = count;
bbStatus = DSHOT_BITBANG_STATUS_OK;
@ -731,7 +734,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
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

View file

@ -34,7 +34,7 @@
#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"

View file

@ -152,7 +152,9 @@ MCU_COMMON_SRC = \
APM32/startup/system_apm32f4xx.c \
drivers/inverter.c \
drivers/dshot_bitbang_decode.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 \
APM32/bus_spi_apm32.c \
APM32/bus_i2c_apm32.c \
APM32/bus_i2c_apm32_init.c \
@ -206,6 +208,8 @@ MSC_SRC = \
msc/usbd_storage_sdio.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

View file

@ -153,7 +153,7 @@ static motorVTable_t motorPwmVTable = {
.convertMotorToExternal = pwmConvertToExternal,
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
{
memset(motors, 0, sizeof(motors));
@ -161,36 +161,36 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
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;
useUnsyncedUpdate = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
case MOTOR_PROTOCOL_STANDARD:
sMin = 1e-3f;
sLen = 1e-3f;
useUnsyncedPwm = true;
useUnsyncedUpdate = true;
idlePulse = 0;
break;
}
motorPwmDevice.vTable.write = pwmWriteStandard;
motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
@ -212,23 +212,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 = useUnsyncedUpdate ? 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 = useUnsyncedUpdate ? 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++) {

View file

@ -33,13 +33,13 @@
#include "drivers/dma.h"
#include "drivers/dma_reqmap.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "dshot_dpwm.h"
#include "drivers/dshot_command.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/motor.h"
#include "drivers/pwm_output.h"
#include "drivers/pwm_output_dshot_shared.h"
#include "pwm_output_dshot_shared.h"
#include "drivers/rcc.h"
#include "drivers/time.h"
#include "drivers/timer.h"
@ -187,7 +187,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)
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output)
{
#ifdef USE_DSHOT_TELEMETRY
#define OCINIT motor->ocInitStruct
@ -275,7 +275,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
DDL_TMR_DisableCounter(timer);
init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
init.Autoreload = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1;
init.Autoreload = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1;
init.ClockDivision = DDL_TMR_CLOCKDIVISION_DIV1;
init.RepetitionCounter = 0;
init.CounterMode = DDL_TMR_COUNTERMODE_UP;
@ -355,7 +355,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
DMAINIT.FIFOMode = DDL_DMA_FIFOMODE_ENABLE;
DMAINIT.MemBurst = DDL_DMA_MBURST_SINGLE;
DMAINIT.PeriphBurst = DDL_DMA_PBURST_SINGLE;
DMAINIT.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
DMAINIT.NbData = pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
DMAINIT.PeriphOrM2MSrcIncMode = DDL_DMA_PERIPH_NOINCREMENT;
DMAINIT.MemoryOrM2MDstIncMode = DDL_DMA_MEMORY_INCREMENT;
DMAINIT.PeriphOrM2MSrcDataSize = DDL_DMA_PDATAALIGN_WORD;
@ -373,7 +373,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);

View file

@ -35,12 +35,12 @@
#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 "pwm_output_dshot_shared.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 "drivers/dshot_bitbang_decode.h"
#include "drivers/time.h"
#include "drivers/timer.h"
@ -93,7 +93,7 @@ const timerHardware_t bbTimerHardware[] = {
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
@ -243,15 +243,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;
}
}
@ -364,7 +364,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);
@ -382,7 +382,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);
@ -669,7 +669,7 @@ static void bbPostInit(void)
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) {
if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) {
return;
}
@ -695,6 +695,8 @@ static motorVTable_t bbVTable = {
.convertExternalToMotor = dshotConvertFromExternal,
.convertMotorToExternal = dshotConvertToExternal,
.shutdown = bbShutdown,
.isMotorIdle = bbDshotIsMotorIdle,
.requestTelemetry = bbDshotRequestTelemetry,
};
dshotBitbangStatus_e dshotBitbangGetStatus(void)
@ -707,7 +709,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
dbgPinLo(0);
dbgPinLo(1);
motorPwmProtocol = motorConfig->motorPwmProtocol;
motorProtocol = motorConfig->motorProtocol;
bbDevice.vTable = bbVTable;
motorCount = count;
bbStatus = DSHOT_BITBANG_STATUS_OK;
@ -723,7 +725,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
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

View file

@ -36,7 +36,7 @@
#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"

View file

@ -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

View file

@ -149,7 +149,7 @@ static motorVTable_t motorPwmVTable = {
.convertMotorToExternal = pwmConvertToExternal,
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
{
memset(motors, 0, sizeof(motors));
@ -157,36 +157,36 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
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;
useUnsyncedUpdate = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
case MOTOR_PROTOCOL_STANDARD:
sMin = 1e-3f;
sLen = 1e-3f;
useUnsyncedPwm = true;
useUnsyncedUpdate = true;
idlePulse = 0;
break;
}
motorPwmDevice.vTable.write = pwmWriteStandard;
motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
@ -208,23 +208,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 = useUnsyncedUpdate ? 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 = useUnsyncedUpdate ? 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++) {

View file

@ -39,9 +39,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
@ -130,7 +130,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
@ -360,7 +360,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
@ -450,7 +450,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);
@ -522,7 +522,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;
@ -550,7 +550,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);

View file

@ -627,10 +627,10 @@ static motorDevice_t motorPwmDevice = {
}
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
{
UNUSED(motorConfig);
UNUSED(useUnsyncedPwm);
UNUSED(useUnsyncedUpdate);
printf("Initialized motor count %d\n", motorCount);
pwmRawPkt.motorCount = motorCount;

View file

@ -35,12 +35,12 @@
#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 "pwm_output_dshot_shared.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 "drivers/dshot_bitbang_decode.h"
#include "drivers/time.h"
#include "drivers/timer.h"
@ -137,7 +137,7 @@ const timerHardware_t bbTimerHardware[] = {
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
@ -287,15 +287,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;
}
}
@ -408,7 +408,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);
@ -426,7 +426,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)
{
// Return if no GPIO is specified
if (!io) {
@ -608,11 +608,6 @@ static void bbWriteInt(uint8_t motorIndex, uint16_t value)
return;
}
// fetch requestTelemetry from motors. Needs to be refactored.
motorDmaOutput_t * const motor = getMotorDmaOutput(motorIndex);
bbmotor->protocolControl.requestTelemetry = motor->protocolControl.requestTelemetry;
motor->protocolControl.requestTelemetry = false;
// If there is a command ready to go overwrite the value and send that instead
if (dshotCommandIsProcessing()) {
value = dshotCommandGetCurrent(motorIndex);
@ -714,7 +709,7 @@ static void bbPostInit(void)
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) {
if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorProtocol, bbMotors[motorIndex].output)) {
return;
}
@ -740,6 +735,8 @@ static motorVTable_t bbVTable = {
.convertExternalToMotor = dshotConvertFromExternal,
.convertMotorToExternal = dshotConvertToExternal,
.shutdown = bbShutdown,
.isMotorIdle = bbDshotIsMotorIdle,
.requestTelemetry = bbDshotRequestTelemetry,
};
dshotBitbangStatus_e dshotBitbangGetStatus(void)
@ -752,7 +749,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
dbgPinLo(0);
dbgPinLo(1);
motorPwmProtocol = motorConfig->motorPwmProtocol;
motorProtocol = motorConfig->motorProtocol;
bbDevice.vTable = bbVTable;
motorCount = count;
bbStatus = DSHOT_BITBANG_STATUS_OK;
@ -768,7 +765,7 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
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

View file

@ -34,13 +34,14 @@
#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"
#include "pwm_output_dshot_shared.h"
#include "pg/motor.h"

View file

@ -35,7 +35,7 @@
#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"

View file

@ -175,7 +175,6 @@ MCU_COMMON_SRC = \
drivers/accgyro/accgyro_mpu.c \
drivers/dshot_bitbang_decode.c \
drivers/inverter.c \
drivers/pwm_output_dshot_shared.c \
STM32/pwm_output_dshot.c \
STM32/adc_stm32f4xx.c \
STM32/bus_i2c_stm32f4xx.c \

View file

@ -143,7 +143,6 @@ MCU_COMMON_SRC = \
drivers/accgyro/accgyro_mpu.c \
drivers/bus_i2c_timing.c \
drivers/dshot_bitbang_decode.c \
drivers/pwm_output_dshot_shared.c \
STM32/adc_stm32f7xx.c \
STM32/audio_stm32f7xx.c \
STM32/bus_i2c_hal_init.c \
@ -191,7 +190,6 @@ SPEED_OPTIMISED_SRC += \
STM32/bus_i2c_hal.c \
STM32/bus_spi_ll.c \
drivers/max7456.c \
drivers/pwm_output_dshot_shared.c \
STM32/pwm_output_dshot_hal.c \
STM32/exti.c

View file

@ -120,7 +120,6 @@ MCU_COMMON_SRC = \
drivers/accgyro/accgyro_mpu.c \
drivers/bus_i2c_timing.c \
drivers/dshot_bitbang_decode.c \
drivers/pwm_output_dshot_shared.c \
STM32/adc_stm32g4xx.c \
STM32/bus_i2c_hal_init.c \
STM32/bus_i2c_hal.c \

View file

@ -151,7 +151,6 @@ MCU_COMMON_SRC = \
drivers/bus_i2c_timing.c \
drivers/bus_quadspi.c \
drivers/dshot_bitbang_decode.c \
drivers/pwm_output_dshot_shared.c \
STM32/bus_i2c_hal_init.c \
STM32/bus_i2c_hal.c \
STM32/bus_spi_ll.c \

View file

@ -266,7 +266,6 @@ MCU_COMMON_SRC = \
drivers/bus_i2c_timing.c \
drivers/bus_quadspi.c \
drivers/dshot_bitbang_decode.c \
drivers/pwm_output_dshot_shared.c \
STM32/adc_stm32h7xx.c \
STM32/audio_stm32h7xx.c \
STM32/bus_i2c_hal_init.c \

View file

@ -7,7 +7,11 @@ MCU_COMMON_SRC += \
common/stm32/bus_spi_pinconfig.c \
common/stm32/bus_spi_hw.c \
common/stm32/io_impl.c \
common/stm32/serial_uart_hw.c
common/stm32/serial_uart_hw.c \
common/stm32/dshot_dpwm.c \
common/stm32/pwm_output_dshot_shared.c \
common/stm32/dshot_bitbang_shared.c
SIZE_OPTIMISED_SRC += \
common/stm32/config_flash.c \
@ -16,5 +20,7 @@ SIZE_OPTIMISED_SRC += \
SPEED_OPTIMISED_SRC += \
common/stm32/system.c \
common/stm32/bus_spi_hw.c \
common/stm32/pwm_output_dshot_shared.c \
common/stm32/dshot_bitbang_shared.c \
common/stm32/io_impl.c

View file

@ -181,7 +181,7 @@ static motorVTable_t motorPwmVTable = {
.convertMotorToExternal = pwmConvertToExternal,
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
{
memset(motors, 0, sizeof(motors));
@ -189,36 +189,36 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
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;
useUnsyncedUpdate = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
case MOTOR_PROTOCOL_STANDARD:
sMin = 1e-3f;
sLen = 1e-3f;
useUnsyncedPwm = true;
useUnsyncedUpdate = true;
idlePulse = 0;
break;
}
motorPwmDevice.vTable.write = pwmWriteStandard;
motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedPwm ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
@ -240,23 +240,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 = useUnsyncedUpdate ? 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 = useUnsyncedUpdate ? 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++) {

View file

@ -42,9 +42,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"
#ifdef USE_DSHOT_TELEMETRY
@ -200,7 +200,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)
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output)
{
#ifdef USE_DSHOT_TELEMETRY
#define OCINIT motor->ocInitStruct
@ -299,7 +299,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
TIM_Cmd(timer, DISABLE);
TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
TIM_TimeBaseStructure.TIM_Period = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
TIM_TimeBaseStructure.TIM_Period = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? (MOTOR_NIBBLE_LENGTH_PROSHOT) : MOTOR_BITLENGTH) - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
@ -359,7 +359,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;
@ -396,7 +396,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);

View file

@ -33,13 +33,13 @@
#include "drivers/dma.h"
#include "drivers/dma_reqmap.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "dshot_dpwm.h"
#include "drivers/dshot_command.h"
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/motor.h"
#include "drivers/pwm_output.h"
#include "drivers/pwm_output_dshot_shared.h"
#include "pwm_output_dshot_shared.h"
#include "drivers/rcc.h"
#include "drivers/time.h"
#include "drivers/timer.h"
@ -200,7 +200,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)
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output)
{
#ifdef USE_DSHOT_TELEMETRY
#define OCINIT motor->ocInitStruct
@ -297,7 +297,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
LL_TIM_DisableCounter(timer);
init.Prescaler = (uint16_t)(lrintf((float) timerClock(timer) / getDshotHz(pwmProtocolType) + 0.01f) - 1);
init.Autoreload = (pwmProtocolType == PWM_TYPE_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1;
init.Autoreload = (pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? MOTOR_NIBBLE_LENGTH_PROSHOT : MOTOR_BITLENGTH) - 1;
init.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
init.RepetitionCounter = 0;
init.CounterMode = LL_TIM_COUNTERMODE_UP;
@ -391,7 +391,7 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
DMAINIT.MemBurst = LL_DMA_MBURST_SINGLE;
DMAINIT.PeriphBurst = LL_DMA_PBURST_SINGLE;
#endif
DMAINIT.NbData = pwmProtocolType == PWM_TYPE_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
DMAINIT.NbData = pwmProtocolType == MOTOR_PROTOCOL_PROSHOT1000 ? PROSHOT_DMA_BUFFER_SIZE : DSHOT_DMA_BUFFER_SIZE;
DMAINIT.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
DMAINIT.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
DMAINIT.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_WORD;
@ -409,7 +409,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);

View file

@ -57,7 +57,7 @@
#define USE_BEEPER
#define DEFAULT_MOTOR_DSHOT_SPEED PWM_TYPE_DSHOT300
#define DEFAULT_MOTOR_DSHOT_SPEED MOTOR_PROTOCOL_DSHOT300
#ifdef USE_SDCARD
#define USE_SDCARD_SPI

View file

@ -22,6 +22,7 @@
#include "common/time.h"
#include "drivers/motor.h"
#include "drivers/dshot.h"
#define USE_DMA_REGISTER_CACHE
@ -275,3 +276,6 @@ void bbDMA_Cmd(bbPort_t *bbPort, confirm_state NewState);
void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState);
#endif
int bbDMA_Count(bbPort_t *bbPort);
void bbDshotRequestTelemetry(uint8_t motorIndex);
bool bbDshotIsMotorIdle(uint8_t motorIndex);

View file

@ -0,0 +1,38 @@
/*
* 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 "dshot_bitbang_impl.h"
void bbDshotRequestTelemetry(uint8_t motorIndex)
{
bbMotor_t *const bbmotor = &bbMotors[motorIndex];
if (!bbmotor->configured) {
return;
}
bbmotor->protocolControl.requestTelemetry = true;
}
bool bbDshotIsMotorIdle(uint8_t motorIndex)
{
bbMotor_t *const bbmotor = &bbMotors[motorIndex];
return bbmotor->protocolControl.value;
}

View file

@ -30,8 +30,9 @@
#ifdef USE_DSHOT
#include "drivers/pwm_output.h"
#include "pwm_output_dshot_shared.h"
#include "drivers/dshot.h"
#include "drivers/dshot_dpwm.h"
#include "dshot_dpwm.h"
#include "drivers/motor.h"
#include "pg/motor.h"
@ -79,17 +80,17 @@ FAST_CODE_NOINLINE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride,
return PROSHOT_DMA_BUFFER_SIZE;
}
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
uint32_t getDshotHz(motorProtocolTypes_e pwmProtocolType)
{
switch (pwmProtocolType) {
case(PWM_TYPE_PROSHOT1000):
case(MOTOR_PROTOCOL_PROSHOT1000):
return MOTOR_PROSHOT1000_HZ;
case(PWM_TYPE_DSHOT600):
case(MOTOR_PROTOCOL_DSHOT600):
return MOTOR_DSHOT600_HZ;
case(PWM_TYPE_DSHOT300):
case(MOTOR_PROTOCOL_DSHOT300):
return MOTOR_DSHOT300_HZ;
default:
case(PWM_TYPE_DSHOT150):
case(MOTOR_PROTOCOL_DSHOT150):
return MOTOR_DSHOT150_HZ;
}
}
@ -147,14 +148,16 @@ static motorVTable_t dshotPwmVTable = {
.convertExternalToMotor = dshotConvertFromExternal,
.convertMotorToExternal = dshotConvertToExternal,
.shutdown = dshotPwmShutdown,
.requestTelemetry = pwmDshotRequestTelemetry,
.isMotorIdle = pwmDshotIsMotorIdle,
};
FAST_DATA_ZERO_INIT motorDevice_t dshotPwmDevice;
motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
{
UNUSED(idlePulse);
UNUSED(useUnsyncedPwm);
UNUSED(useUnsyncedUpdate);
dshotPwmDevice.vTable = dshotPwmVTable;
@ -163,13 +166,13 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
dshotPwmDevice.vTable.decodeTelemetry = pwmTelemetryDecode;
#endif
switch (motorConfig->motorPwmProtocol) {
case PWM_TYPE_PROSHOT1000:
switch (motorConfig->motorProtocol) {
case MOTOR_PROTOCOL_PROSHOT1000:
loadDmaBuffer = loadDmaBufferProshot;
break;
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
case MOTOR_PROTOCOL_DSHOT600:
case MOTOR_PROTOCOL_DSHOT300:
case MOTOR_PROTOCOL_DSHOT150:
loadDmaBuffer = loadDmaBufferDshot;
#ifdef USE_DSHOT_DMAR
useBurstDshot = motorConfig->useBurstDshot == DSHOT_DMAR_ON ||
@ -190,8 +193,8 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
if (pwmDshotMotorHardwareConfig(timerHardware,
motorIndex,
reorderedMotorIndex,
motorConfig->motorPwmProtocol,
motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) {
motorConfig->motorProtocol,
motorConfig->motorInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) {
motors[motorIndex].enabled = true;
continue;

View file

@ -23,7 +23,7 @@
#pragma once
#include "drivers/dshot.h"
#include "drivers/motor.h"
#include "drivers/motor_types.h"
// Timer clock frequency for the dshot speeds
#define MOTOR_DSHOT600_HZ MHZ_TO_HZ(12)
@ -47,10 +47,7 @@ extern FAST_DATA_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet);
uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet);
uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType);
struct motorDevConfig_s;
motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm);
uint32_t getDshotHz(motorProtocolTypes_e pwmProtocolType);
/* Motor DMA related, moved from pwm_output.h */
@ -70,7 +67,7 @@ motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint1
#elif defined(STM32F7)
#define DSHOT_DMA_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT
#else
#define DSHOT_DMA_BUFFER_ATTRIBUTE // None
#define DSHOT_DMA_BUFFER_ATTRIBUTE /* Empty */
#endif
#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) || defined(STM32G4) || defined(AT32F435) || defined(APM32F4)
@ -159,7 +156,7 @@ typedef struct motorDmaOutput_s {
motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
void pwmWriteDshotInt(uint8_t index, uint16_t value);
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output);
bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output);
#ifdef USE_DSHOT_TELEMETRY
bool pwmTelemetryDecode(void);
#endif

View file

@ -40,9 +40,9 @@
#include "stm32f4xx.h"
#endif
#include "pwm_output.h"
#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/motor.h"
@ -60,7 +60,7 @@ motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
#ifdef USE_DSHOT_TELEMETRY
FAST_DATA_ZERO_INIT uint32_t inputStampUs;
FAST_DATA_ZERO_INIT dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters;
FAST_DATA_ZERO_INIT dshotTelemetryCycleCounters_t dshotDMAHandlerCycleCounters;
#endif
motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
@ -68,6 +68,18 @@ motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
return &dmaMotors[index];
}
bool pwmDshotIsMotorIdle(uint8_t index)
{
const motorDmaOutput_t *motor = getMotorDmaOutput(index);
return motor->protocolControl.value;
}
void pwmDshotRequestTelemetry(uint8_t index)
{
motorDmaOutput_t * const motor = getMotorDmaOutput(index);
motor->protocolControl.requestTelemetry = true;
}
uint8_t getTimerIndex(TIM_TypeDef *timer)
{
for (int i = 0; i < dmaMotorTimerCount; i++) {
@ -127,12 +139,12 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
#else
xDMA_SetCurrDataCounter(motor->dmaRef, bufferSize);
// XXX we can remove this ifdef if we add a new macro for the TRUE/ENABLE constants
#ifdef AT32F435
// XXX we can remove this ifdef if we add a new macro for the TRUE/ENABLE constants
#ifdef AT32F435
xDMA_Cmd(motor->dmaRef, TRUE);
#else
#else
xDMA_Cmd(motor->dmaRef, ENABLE);
#endif
#endif
#endif // USE_FULL_LL_DRIVER
}

View file

@ -20,28 +20,13 @@
#ifdef USE_DSHOT
#include "drivers/dshot.h"
#include "dshot_dpwm.h"
extern FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount;
#if defined(STM32F7) || defined(STM32H7)
extern FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
extern FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
#else
extern motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
extern motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
#endif
#ifdef USE_DSHOT_TELEMETRY
extern uint32_t readDoneCount;
FAST_DATA_ZERO_INIT extern uint32_t inputStampUs;
typedef struct dshotDMAHandlerCycleCounters_s {
uint32_t irqAt;
uint32_t changeDirectionCompletedAt;
} dshotDMAHandlerCycleCounters_t;
FAST_DATA_ZERO_INIT extern dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters;
#endif
uint8_t getTimerIndex(TIM_TypeDef *timer);
motorDmaOutput_t *getMotorDmaOutput(uint8_t index);
@ -59,6 +44,8 @@ void pwmDshotSetDirectionOutput(
#endif
);
void pwmDshotRequestTelemetry(uint8_t index);
bool pwmDshotIsMotorIdle(uint8_t index);
bool pwmTelemetryDecode(void);
#endif

View file

@ -37,9 +37,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.dev = {
.motorPwmRate = 400,
.motorPwmProtocol = 0,
.motorPwmInversion = 0,
.useUnsyncedPwm = 0,
.motorProtocol = 0,
.motorInversion = 0,
.useUnsyncedUpdate = 0,
.useBurstDshot = 0,
.useDshotTelemetry = 0,
.useDshotEdt = 0,