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:
parent
c513b102b6
commit
d3c113b4c5
56 changed files with 428 additions and 321 deletions
|
@ -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);
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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)},
|
||||
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
72
src/main/drivers/motor_types.h
Normal file
72
src/main/drivers/motor_types.h
Normal 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;
|
|
@ -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 {
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -20,8 +20,6 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "drivers/pwm_output.h"
|
||||
|
||||
#define ESCSERIAL_BUFFER_SIZE 1024
|
||||
|
||||
typedef enum {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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"
|
||||
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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++) {
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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++) {
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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"
|
||||
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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 \
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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 \
|
||||
|
|
|
@ -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 \
|
||||
|
|
|
@ -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 \
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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++) {
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
38
src/platform/common/stm32/dshot_bitbang_shared.c
Normal file
38
src/platform/common/stm32/dshot_bitbang_shared.c
Normal 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;
|
||||
}
|
|
@ -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;
|
|
@ -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
|
|
@ -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
|
||||
}
|
|
@ -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
|
|
@ -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,
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue