Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 11:29:58 +03:00
Code Issues Wiki Activity

Refactored motor to use only one motorDevice_t instance, and vTable is now pointing to const.

Browse source
This commit is contained in:
blckmn 2025-01-17 05:06:03 +11:00
parent bfc9680509
commit a9cf384409
30 changed files with 411 additions and 377 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/config/config.c
View file

@ -284,7 +284,7 @@ static void validateAndFixConfig(void)
}
}
if ((motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_PWM50HZ ) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
if ((motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_PWM ) && (motorConfig()->dev.motorPwmRate > BRUSHLESS_MOTORS_PWM_RATE)) {
motorConfigMutable()->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
}
@ -598,7 +598,7 @@ void validateAndFixGyroConfig(void)
}
#endif // USE_DSHOT && USE_PID_DENOM_CHECK
switch (motorConfig()->dev.motorProtocol) {
case MOTOR_PROTOCOL_PWM50HZ :
case MOTOR_PROTOCOL_PWM :
motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE;
break;
case MOTOR_PROTOCOL_ONESHOT125:
@ -624,7 +624,7 @@ void validateAndFixGyroConfig(void)
bool configuredMotorProtocolDshot = false;
checkMotorProtocolEnabled(&motorConfig()->dev, &configuredMotorProtocolDshot);
// Prevent overriding the max rate of motors
if (!configuredMotorProtocolDshot && motorConfig()->dev.motorProtocol != MOTOR_PROTOCOL_PWM50HZ ) {
if (!configuredMotorProtocolDshot && motorConfig()->dev.motorProtocol != MOTOR_PROTOCOL_PWM ) {
const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
}

5
src/main/drivers/dshot.h
View file

@ -27,6 +27,8 @@
#include "pg/motor.h"
#include "drivers/motor_types.h"
// TODO: move bitbang as implementation detail of dshot (i.e. dshot should be the interface)
#include "drivers/dshot_bitbang.h"
#define DSHOT_MIN_THROTTLE (48)
#define DSHOT_MAX_THROTTLE (2047)
@ -124,8 +126,7 @@ FAST_DATA_ZERO_INIT extern dshotTelemetryCycleCounters_t dshotDMAHandlerCycleCou
#endif
struct motorDevConfig_s;
motorDevice_t *dshotPwmDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate);
void dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig);
extern dshotTelemetryState_t dshotTelemetryState;

17
src/main/drivers/dshot_bitbang.h
View file

@ -20,13 +20,16 @@
#pragma once
#include "platform.h"
#include "common/time.h"
#include "drivers/dma.h"
#include "drivers/io_types.h"
#include "drivers/motor_types.h"
#include "drivers/timer.h"
typedef enum {
DSHOT_BITBANG_OFF,
DSHOT_BITBANG_ON,
DSHOT_BITBANG_AUTO,
} dshotBitbangMode_e;
#include "pg/motor.h"
typedef enum {
DSHOT_BITBANG_STATUS_OK,
@ -35,9 +38,7 @@ typedef enum {
DSHOT_BITBANG_STATUS_TOO_MANY_PORTS,
} dshotBitbangStatus_e;
struct motorDevConfig_s;
struct motorDevice_s;
struct motorDevice_s *dshotBitbangDevInit(const struct motorDevConfig_s *motorConfig, uint8_t motorCount);
void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig);
dshotBitbangStatus_e dshotBitbangGetStatus();
const timerHardware_t *dshotBitbangTimerGetAllocatedByNumberAndChannel(int8_t timerNumber, uint16_t timerChannel);
const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer);

6
src/main/drivers/dshot_command.c
View file

@ -177,7 +177,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
uint8_t repeats = 1;
timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
motorVTable_t *vTable = motorGetVTable();
const motorVTable_t *vTable = motorGetVTable();
switch (command) {
case DSHOT_CMD_SPIN_DIRECTION_1:
@ -274,7 +274,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
}
}
uint8_t dshotCommandGetCurrent(uint8_t index)
uint8_t dshotCommandGetCurrent(unsigned index)
{
return commandQueue[commandQueueTail].command[index];
}
@ -284,7 +284,7 @@ uint8_t dshotCommandGetCurrent(uint8_t index)
// allows the motor output to be sent, "false" means delay until next loop. So take
// the example of a dshot command that needs to repeat 10 times at 1ms intervals.
// If we have a 8KHz PID loop we'll end up sending the dshot command every 8th motor output.
FAST_CODE_NOINLINE bool dshotCommandOutputIsEnabled(uint8_t motorCount)
FAST_CODE_NOINLINE bool dshotCommandOutputIsEnabled(unsigned motorCount)
{
UNUSED(motorCount);

4
src/main/drivers/dshot_command.h
View file

@ -71,6 +71,6 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
void dshotSetPidLoopTime(uint32_t pidLoopTime);
bool dshotCommandQueueEmpty(void);
bool dshotCommandIsProcessing(void);
uint8_t dshotCommandGetCurrent(uint8_t index);
bool dshotCommandOutputIsEnabled(uint8_t motorCount);
uint8_t dshotCommandGetCurrent(unsigned index);
bool dshotCommandOutputIsEnabled(unsigned motorCount);
bool dshotStreamingCommandsAreEnabled(void);

307
src/main/drivers/motor.c
View file

@ -44,7 +44,7 @@
#include "motor.h"
static FAST_DATA_ZERO_INIT motorDevice_t *motorDevice;
static FAST_DATA_ZERO_INIT motorDevice_t motorDevice;
static bool motorProtocolEnabled = false;
static bool motorProtocolDshot = false;
@ -52,13 +52,13 @@ static bool motorProtocolDshot = false;
void motorShutdown(void)
{
uint32_t shutdownDelayUs = 1500;
motorDevice->vTable.shutdown();
motorDevice->enabled = false;
motorDevice->motorEnableTimeMs = 0;
motorDevice->initialized = false;
motorDevice.vTable->shutdown();
motorDevice.enabled = false;
motorDevice.motorEnableTimeMs = 0;
motorDevice.initialized = false;
switch (motorConfig()->dev.motorProtocol) {
case MOTOR_PROTOCOL_PWM50HZ :
case MOTOR_PROTOCOL_PWM :
case MOTOR_PROTOCOL_ONESHOT125:
case MOTOR_PROTOCOL_ONESHOT42:
case MOTOR_PROTOCOL_MULTISHOT:
@ -75,31 +75,33 @@ void motorShutdown(void)
void motorWriteAll(float *values)
{
#ifdef USE_PWM_OUTPUT
if (motorDevice->enabled) {
if (motorDevice.enabled) {
#ifdef USE_DSHOT_BITBANG
if (isDshotBitbangActive(&motorConfig()->dev)) {
// Initialise the output buffers
if (motorDevice->vTable.updateInit) {
motorDevice->vTable.updateInit();
if (motorDevice.vTable->updateInit) {
motorDevice.vTable->updateInit();
}
// Update the motor data
for (int i = 0; i < motorDevice->count; i++) {
motorDevice->vTable.write(i, values[i]);
for (int i = 0; i < motorDevice.count; i++) {
motorDevice.vTable->write(i, values[i]);
}
// Don't attempt to write commands to the motors if telemetry is still being received
if (motorDevice->vTable.telemetryWait) {
(void)motorDevice->vTable.telemetryWait();
if (motorDevice.vTable->telemetryWait) {
(void)motorDevice.vTable->telemetryWait();
}
// Trigger the transmission of the motor data
motorDevice->vTable.updateComplete();
motorDevice.vTable->updateComplete();
// Perform the decode of the last data received
// New data will be received once the send of motor data, triggered above, completes
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
motorDevice->vTable.decodeTelemetry();
if (motorDevice.vTable->decodeTelemetry) {
motorDevice.vTable->decodeTelemetry();
}
#endif
} else
#endif
@ -107,17 +109,16 @@ void motorWriteAll(float *values)
// Perform the decode of the last data received
// New data will be received once the send of motor data, triggered above, completes
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
motorDevice->vTable.decodeTelemetry();
motorDevice.vTable->decodeTelemetry();
#endif
// Update the motor data
for (int i = 0; i < motorDevice->count; i++) {
motorDevice->vTable.write(i, values[i]);
for (int i = 0; i < motorDevice.count; i++) {
motorDevice.vTable->write(i, values[i]);
}
// Trigger the transmission of the motor data
motorDevice->vTable.updateComplete();
motorDevice.vTable->updateComplete();
}
}
#else
@ -127,23 +128,23 @@ void motorWriteAll(float *values)
void motorRequestTelemetry(unsigned index)
{
if (index >= motorDevice->count) {
if (index >= motorDevice.count) {
return;
}
if (motorDevice->vTable.requestTelemetry) {
motorDevice->vTable.requestTelemetry(index);
if (motorDevice.vTable && motorDevice.vTable->requestTelemetry) {
motorDevice.vTable->requestTelemetry(index);
}
}
unsigned motorDeviceCount(void)
{
return motorDevice->count;
return motorDevice.count;
}
motorVTable_t *motorGetVTable(void)
const motorVTable_t *motorGetVTable(void)
{
return &motorDevice->vTable;
return motorDevice.vTable;
}
// This is not motor generic anymore; should be moved to analog pwm module
@ -170,7 +171,7 @@ bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isP
bool isDshot = false;
switch (motorDevConfig->motorProtocol) {
case MOTOR_PROTOCOL_PWM50HZ :
case MOTOR_PROTOCOL_PWM :
case MOTOR_PROTOCOL_ONESHOT125:
case MOTOR_PROTOCOL_ONESHOT42:
case MOTOR_PROTOCOL_MULTISHOT:
@ -222,19 +223,147 @@ void motorInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, flo
float motorConvertFromExternal(uint16_t externalValue)
{
return motorDevice->vTable.convertExternalToMotor(externalValue);
return motorDevice.vTable->convertExternalToMotor(externalValue);
}
uint16_t motorConvertToExternal(float motorValue)
{
return motorDevice->vTable.convertMotorToExternal(motorValue);
return motorDevice.vTable->convertMotorToExternal(motorValue);
}
void motorPostInit(void)
{
motorDevice->vTable.postInit();
if (motorDevice.vTable && motorDevice.vTable->postInit) {
motorDevice.vTable->postInit();
}
}
bool isMotorProtocolEnabled(void)
{
return motorProtocolEnabled;
}
bool isMotorProtocolDshot(void)
{
return motorProtocolDshot;
}
bool isMotorProtocolBidirDshot(void)
{
return isMotorProtocolDshot() && useDshotTelemetry;
}
void motorNullDevInit(motorDevice_t *device);
void motorDevInit(unsigned motorCount)
{
#if defined(USE_PWM_OUTPUT) || defined(USE_DSHOT)
const motorDevConfig_t *motorDevConfig = &motorConfig()->dev;
#endif
#if defined(USE_PWM_OUTPUT)
uint16_t idlePulse = motorConfig()->mincommand;
if (featureIsEnabled(FEATURE_3D)) {
idlePulse = flight3DConfig()->neutral3d;
}
if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_BRUSHED) {
idlePulse = 0; // brushed motors
}
#endif
motorDevice.count = motorCount;
if (isMotorProtocolEnabled()) {
do {
if (!isMotorProtocolDshot()) {
#ifdef USE_PWM_OUTPUT
motorPwmDevInit(&motorDevice, motorDevConfig, idlePulse);
#endif
break;
}
#ifdef USE_DSHOT
#ifdef USE_DSHOT_BITBANG
if (isDshotBitbangActive(motorDevConfig)) {
dshotBitbangDevInit(&motorDevice, motorDevConfig);
break;
}
#endif
dshotPwmDevInit(&motorDevice, motorDevConfig);
#endif
} while(0);
}
// if the VTable has been populated, the device is initialized.
if (motorDevice.vTable) {
motorDevice.initialized = true;
motorDevice.motorEnableTimeMs = 0;
motorDevice.enabled = false;
} else {
motorNullDevInit(&motorDevice);
}
}
void motorDisable(void)
{
motorDevice.vTable->disable();
motorDevice.enabled = false;
motorDevice.motorEnableTimeMs = 0;
}
void motorEnable(void)
{
if (motorDevice.initialized && motorDevice.vTable && motorDevice.vTable->enable()) {
motorDevice.enabled = true;
motorDevice.motorEnableTimeMs = millis();
}
}
float motorEstimateMaxRpm(void)
{
// Empirical testing found this relationship between estimated max RPM without props attached
// (unloaded) and measured max RPM with props attached (loaded), independent from prop size
float unloadedMaxRpm = 0.01f * getBatteryVoltage() * motorConfig()->kv;
float loadDerating = -5.44e-6f * unloadedMaxRpm + 0.944f;
return unloadedMaxRpm * loadDerating;
}
bool motorIsEnabled(void)
{
return motorDevice.enabled;
}
bool motorIsMotorEnabled(unsigned index)
{
return motorDevice.vTable->isMotorEnabled(index);
}
bool motorIsMotorIdle(unsigned index)
{
return motorDevice.vTable->isMotorIdle ? motorDevice.vTable->isMotorIdle(index) : false;
}
#ifdef USE_DSHOT
timeMs_t motorGetMotorEnableTimeMs(void)
{
return motorDevice.motorEnableTimeMs;
}
#endif
//TODO: Remove this function
#ifdef USE_DSHOT_BITBANG
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->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000);
#else
return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
(motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000);
#endif
}
#endif
/* functions below for empty methods and no active motors */
void motorPostInitNull(void)
{
}
@ -307,123 +436,11 @@ static const motorVTable_t motorNullVTable = {
.isMotorIdle = NULL,
};
static motorDevice_t motorNullDevice = {
.initialized = false,
.enabled = false,
};
bool isMotorProtocolEnabled(void)
void motorNullDevInit(motorDevice_t *device)
{
return motorProtocolEnabled;
device->vTable = &motorNullVTable;
device->count = 0;
}
bool isMotorProtocolDshot(void)
{
return motorProtocolDshot;
}
bool isMotorProtocolBidirDshot(void)
{
return isMotorProtocolDshot() && useDshotTelemetry;
}
void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount)
{
#if defined(USE_PWM_OUTPUT) || defined(USE_DSHOT)
bool useUnsyncedUpdate = motorDevConfig->useUnsyncedUpdate;
#else
UNUSED(idlePulse);
UNUSED(motorDevConfig);
#endif
if (isMotorProtocolEnabled()) {
do {
if (!isMotorProtocolDshot()) {
#ifdef USE_PWM_OUTPUT
motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedUpdate);
#endif
break;
}
#ifdef USE_DSHOT
#ifdef USE_DSHOT_BITBANG
if (isDshotBitbangActive(motorDevConfig)) {
motorDevice = dshotBitbangDevInit(motorDevConfig, motorCount);
break;
}
#endif
motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedUpdate);
#endif
} while(0);
}
if (motorDevice) {
motorDevice->count = motorCount;
motorDevice->initialized = true;
motorDevice->motorEnableTimeMs = 0;
motorDevice->enabled = false;
} else {
motorNullDevice.vTable = motorNullVTable;
motorDevice = &motorNullDevice;
}
}
void motorDisable(void)
{
motorDevice->vTable.disable();
motorDevice->enabled = false;
motorDevice->motorEnableTimeMs = 0;
}
void motorEnable(void)
{
if (motorDevice->initialized && motorDevice->vTable.enable()) {
motorDevice->enabled = true;
motorDevice->motorEnableTimeMs = millis();
}
}
float motorEstimateMaxRpm(void)
{
// Empirical testing found this relationship between estimated max RPM without props attached
// (unloaded) and measured max RPM with props attached (loaded), independent from prop size
float unloadedMaxRpm = 0.01f * getBatteryVoltage() * motorConfig()->kv;
float loadDerating = -5.44e-6f * unloadedMaxRpm + 0.944f;
return unloadedMaxRpm * loadDerating;
}
bool motorIsEnabled(void)
{
return motorDevice->enabled;
}
bool motorIsMotorEnabled(uint8_t index)
{
return motorDevice->vTable.isMotorEnabled(index);
}
bool motorIsMotorIdle(unsigned index)
{
return motorDevice->vTable.isMotorIdle ? motorDevice->vTable.isMotorIdle(index) : false;
}
#ifdef USE_DSHOT
timeMs_t motorGetMotorEnableTimeMs(void)
{
return motorDevice->motorEnableTimeMs;
}
#endif
#ifdef USE_DSHOT_BITBANG
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->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000);
#else
return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
(motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorProtocol != MOTOR_PROTOCOL_PROSHOT1000);
#endif
}
#endif
#endif // USE_MOTOR

7
src/main/drivers/motor.h
View file

@ -42,10 +42,9 @@ void motorInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, flo
float motorConvertFromExternal(uint16_t externalValue);
uint16_t motorConvertToExternal(float motorValue);
struct motorDevConfig_s; // XXX Shouldn't be needed once pwm_output* is really cleaned up.
void motorDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount);
void motorDevInit(unsigned motorCount);
unsigned motorDeviceCount(void);
motorVTable_t *motorGetVTable(void);
const motorVTable_t *motorGetVTable(void);
bool checkMotorProtocolEnabled(const motorDevConfig_t *motorConfig, bool *protocolIsDshot);
bool isMotorProtocolDshot(void);
bool isMotorProtocolBidirDshot(void);
@ -55,7 +54,7 @@ void motorDisable(void);
void motorEnable(void);
float motorEstimateMaxRpm(void);
bool motorIsEnabled(void);
bool motorIsMotorEnabled(uint8_t index);
bool motorIsMotorEnabled(unsigned index);
bool motorIsMotorIdle(unsigned index);
timeMs_t motorGetMotorEnableTimeMs(void);
void motorShutdown(void); // Replaces stopPwmAllMotors

30
src/main/drivers/motor_impl.h Normal file
View file

@ -0,0 +1,30 @@
/*
* 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 "drivers/motor_types.h"
void motorPostInitNull(void);
bool motorDecodeTelemetryNull(void);
void motorUpdateCompleteNull(void);
void motorNullDevInit(motorDevice_t *device);

4
src/main/drivers/motor_types.h
View file

@ -28,7 +28,7 @@
#define MOTOR_OUTPUT_LIMIT_PERCENT_MAX 100
typedef enum {
MOTOR_PROTOCOL_PWM50HZ = 0,
MOTOR_PROTOCOL_PWM = 0,
MOTOR_PROTOCOL_ONESHOT125,
MOTOR_PROTOCOL_ONESHOT42,
MOTOR_PROTOCOL_MULTISHOT,
@ -64,7 +64,7 @@ typedef struct motorVTable_s {
} motorVTable_t;
typedef struct motorDevice_s {
motorVTable_t vTable;
const motorVTable_t *vTable;
uint8_t count;
bool initialized;
bool enabled;

7
src/main/drivers/pwm_output.h
View file

@ -26,9 +26,11 @@
#include "drivers/dma.h"
#include "drivers/io_types.h"
#include "drivers/motor.h"
#include "drivers/motor_types.h"
#include "drivers/timer.h"
#include "pg/motor.h"
#define PWM_TIMER_1MHZ MHZ_TO_HZ(1)
// TODO: move the implementation defintions to impl header (platform)
@ -50,8 +52,7 @@ typedef struct {
extern FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
struct motorDevConfig_s;
motorDevice_t *motorPwmDevInit(const struct motorDevConfig_s *motorDevConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm);
void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorDevConfig, uint16_t idlePulse);
typedef struct servoDevConfig_s {
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)

11
src/main/fc/init.c
View file

@ -540,21 +540,12 @@ void init(void)
mixerInit(mixerConfig()->mixerMode);
uint16_t idlePulse = motorConfig()->mincommand;
if (featureIsEnabled(FEATURE_3D)) {
idlePulse = flight3DConfig()->neutral3d;
}
if (motorConfig()->dev.motorProtocol == MOTOR_PROTOCOL_BRUSHED) {
idlePulse = 0; // brushed motors
}
#ifdef USE_MOTOR
/* Motors needs to be initialized soon as posible because hardware initialization
* may send spurious pulses to esc's causing their early initialization. Also ppm
* receiver may share timer with motors so motors MUST be initialized here. */
motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
motorDevInit(getMotorCount());
systemState |= SYSTEM_STATE_MOTORS_READY;
#else
UNUSED(idlePulse);
#endif
if (0) {}

2
src/main/pg/motor.c
View file

@ -58,7 +58,7 @@ void pgResetFn_motorConfig(motorConfig_t *motorConfig)
#else
motorConfig->dev.motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
#ifndef USE_DSHOT
if (motorConfig->dev.motorProtocol == MOTOR_PROTOCOL_PWM50HZ ) {
if (motorConfig->dev.motorProtocol == MOTOR_PROTOCOL_PWM ) {
motorConfig->dev.useUnsyncedUpdate = true;
}
motorConfig->dev.motorProtocol = MOTOR_PROTOCOL_DISABLED;

7
src/main/pg/motor.h
View file

@ -23,7 +23,6 @@
#include "pg/pg.h"
#include "drivers/io.h"
#include "drivers/dshot_bitbang.h"
#if !defined(BRUSHED_MOTORS_PWM_RATE)
#define BRUSHED_MOTORS_PWM_RATE 16000
@ -47,6 +46,12 @@ typedef enum {
DSHOT_DMAR_AUTO
} dshotDmar_e;
typedef enum {
DSHOT_BITBANG_OFF,
DSHOT_BITBANG_ON,
DSHOT_BITBANG_AUTO,
} dshotBitbangMode_e;
typedef enum {
DSHOT_TELEMETRY_OFF,
DSHOT_TELEMETRY_ON,

27
src/platform/APM32/dshot_bitbang.c
View file

@ -64,7 +64,6 @@ FAST_DATA_ZERO_INIT int usedMotorPorts;
FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS];
static FAST_DATA_ZERO_INIT int motorCount;
dshotBitbangStatus_e bbStatus;
// For MCUs that use MPU to control DMA coherency, there might be a performance hit
@ -104,7 +103,6 @@ const timerHardware_t bbTimerHardware[] = {
#endif
};
static FAST_DATA_ZERO_INIT motorDevice_t bbDevice;
static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
static motorProtocolTypes_e motorProtocol;
@ -428,10 +426,6 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmP
}
if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
return false;
}
@ -612,7 +606,7 @@ static void bbUpdateComplete(void)
// If there is a dshot command loaded up, time it correctly with motor update
if (!dshotCommandQueueEmpty()) {
if (!dshotCommandOutputIsEnabled(bbDevice.count)) {
if (!dshotCommandOutputIsEnabled(motorCount)) {
return;
}
}
@ -707,14 +701,18 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void)
return bbStatus;
}
motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count)
void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)
{
dbgPinLo(0);
dbgPinLo(1);
if (!device || !motorConfig) {
return;
}
motorProtocol = motorConfig->motorProtocol;
bbDevice.vTable = bbVTable;
motorCount = count;
device->vTable = &bbVTable;
motorCount = device->count;
bbStatus = DSHOT_BITBANG_STATUS_OK;
#ifdef USE_DSHOT_TELEMETRY
@ -739,11 +737,10 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
if (!IOIsFreeOrPreinit(io)) {
/* not enough motors initialised for the mixer or a break in the motors */
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
device->vTable = NULL;
motorCount = 0;
bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
return NULL;
return;
}
int pinIndex = IO_GPIOPinIdx(io);
@ -763,8 +760,6 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
IOHi(io);
}
}
return &bbDevice;
}
#endif // USE_DSHOT_BITBANG

71
src/platform/APM32/pwm_output_apm32.c
View file

@ -40,7 +40,9 @@ FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static void pwmOCConfig(TMR_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
TMR_HandleTypeDef* Handle = timerFindTimerHandle(tim);
if (Handle == NULL) return;
if (Handle == NULL) {
return;
}
TMR_OC_InitTypeDef TMR_OCInitStructure;
@ -58,7 +60,9 @@ static void pwmOCConfig(TMR_TypeDef *tim, uint8_t channel, uint16_t value, uint8
void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware, uint32_t hz, uint16_t period, uint16_t value, uint8_t inversion)
{
TMR_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
if (Handle == NULL) return;
if (Handle == NULL) {
return;
}
configTimeBase(timerHardware->tim, period, hz);
pwmOCConfig(timerHardware->tim,
@ -67,10 +71,11 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
inversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output
);
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL)
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
DAL_TMREx_PWMN_Start(Handle, timerHardware->channel);
else
} else {
DAL_TMR_PWM_Start(Handle, timerHardware->channel);
}
DAL_TMR_Base_Start(Handle);
channel->ccr = timerChCCR(timerHardware);
@ -80,13 +85,8 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
*channel->ccr = 0;
}
static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice;
static void pwmWriteUnused(uint8_t index, float value)
{
UNUSED(index);
UNUSED(value);
}
static int motorCount = 0;
static bool useContinuousUpdate = true;
static void pwmWriteStandard(uint8_t index, float value)
{
@ -96,7 +96,7 @@ static void pwmWriteStandard(uint8_t index, float value)
void pwmShutdownPulsesForAllMotors(void)
{
for (int index = 0; index < motorPwmDevice.count; index++) {
for (int index = 0; index < motorCount; index++) {
// Set the compare register to 0, which stops the output pulsing if the timer overflows
if (motors[index].channel.ccr) {
*motors[index].channel.ccr = 0;
@ -113,7 +113,7 @@ static motorVTable_t motorPwmVTable;
bool pwmEnableMotors(void)
{
/* check motors can be enabled */
return (motorPwmVTable.write != &pwmWriteUnused);
return motorCount > 0;
}
bool pwmIsMotorEnabled(unsigned index)
@ -121,9 +121,13 @@ bool pwmIsMotorEnabled(unsigned index)
return motors[index].enabled;
}
static void pwmCompleteOneshotMotorUpdate(void)
static void pwmCompleteMotorUpdate(void)
{
for (int index = 0; index < motorPwmDevice.count; index++) {
if (useContinuousUpdate) {
return;
}
for (int index = 0; index < motorCount; index++) {
if (motors[index].forceOverflow) {
timerForceOverflow(motors[index].channel.tim);
}
@ -144,23 +148,32 @@ static uint16_t pwmConvertToExternal(float motorValue)
}
static motorVTable_t motorPwmVTable = {
.postInit = motorPostInitNull,
.postInit = NULL,
.enable = pwmEnableMotors,
.disable = pwmDisableMotors,
.isMotorEnabled = pwmIsMotorEnabled,
.shutdown = pwmShutdownPulsesForAllMotors,
.convertExternalToMotor = pwmConvertFromExternal,
.convertMotorToExternal = pwmConvertToExternal,
.write = pwmWriteStandard,
.decodeTelemetry = NULL,
.updateComplete = pwmCompleteMotorUpdate,
.requestTelemetry = NULL,
.isMotorIdle = NULL,
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)
{
memset(motors, 0, sizeof(motors));
motorPwmDevice.vTable = motorPwmVTable;
if (!device || !motorConfig) {
return;
}
motorCount = device->count;
device->vTable = &motorPwmVTable;
useContinuousUpdate = motorConfig->useUnsyncedUpdate;
float sMin = 0;
float sLen = 0;
switch (motorConfig->motorProtocol) {
@ -179,21 +192,17 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
break;
case MOTOR_PROTOCOL_BRUSHED:
sMin = 0;
useUnsyncedUpdate = true;
useContinuousUpdate = true;
idlePulse = 0;
break;
case MOTOR_PROTOCOL_PWM50HZ :
case MOTOR_PROTOCOL_PWM :
sMin = 1e-3f;
sLen = 1e-3f;
useUnsyncedUpdate = true;
useContinuousUpdate = true;
idlePulse = 0;
break;
}
motorPwmDevice.vTable.write = pwmWriteStandard;
motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
@ -201,10 +210,10 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
motorPwmDevice.vTable.write = &pwmWriteUnused;
motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
device->vTable = NULL;
motorCount = 0;
/* TODO: block arming and add reason system cannot arm */
return NULL;
return;
}
motors[motorIndex].io = IOGetByTag(tag);
@ -214,13 +223,13 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
/* standard PWM outputs */
// margin of safety is 4 periods when unsynced
const unsigned pwmRateHz = useUnsyncedUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
const uint32_t clock = timerClock(timerHardware->tim);
/* used to find the desired timer frequency for max resolution */
const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
const uint32_t hz = clock / prescaler;
const unsigned period = useUnsyncedUpdate ? hz / pwmRateHz : 0xffff;
const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff;
/*
if brushed then it is the entire length of the period.
@ -242,8 +251,6 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
motors[motorIndex].forceOverflow = !timerAlreadyUsed;
motors[motorIndex].enabled = true;
}
return &motorPwmDevice;
}
pwmOutputPort_t *pwmGetMotors(void)

2
src/platform/APM32/pwm_output_dshot_apm32.c
View file

@ -127,7 +127,7 @@ FAST_CODE static void pwmDshotSetDirectionInput(
FAST_CODE void pwmCompleteDshotMotorUpdate(void)
{
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(motorCount)) {
return;
}

24
src/platform/AT32/dshot_bitbang.c
View file

@ -57,7 +57,6 @@ FAST_DATA_ZERO_INIT int usedMotorPorts;
FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS];
static FAST_DATA_ZERO_INIT int motorCount;
dshotBitbangStatus_e bbStatus;
// For MCUs that use MPU to control DMA coherency, there might be a performance hit
@ -89,7 +88,6 @@ const timerHardware_t bbTimerHardware[] = {
DEF_TIM(TMR1, CH4, NONE, 0, 3, 0),
};
static FAST_DATA_ZERO_INIT motorDevice_t bbDevice;
static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
static motorProtocolTypes_e motorProtocol;
@ -408,10 +406,6 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmP
}
if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
return false;
}
@ -592,7 +586,7 @@ static void bbUpdateComplete(void)
// If there is a dshot command loaded up, time it correctly with motor update
if (!dshotCommandQueueEmpty()) {
if (!dshotCommandOutputIsEnabled(bbDevice.count)) {
if (!dshotCommandOutputIsEnabled(motorCount)) {
return;
}
}
@ -699,14 +693,15 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void)
return bbStatus;
}
motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count)
void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)
{
dbgPinLo(0);
dbgPinLo(1);
motorProtocol = motorConfig->motorProtocol;
bbDevice.vTable = bbVTable;
motorCount = count;
device->vTable = &bbVTable;
motorCount = device->count;
bbStatus = DSHOT_BITBANG_STATUS_OK;
#ifdef USE_DSHOT_TELEMETRY
@ -731,11 +726,10 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
if (!IOIsFreeOrPreinit(io)) {
/* not enough motors initialised for the mixer or a break in the motors */
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
return NULL;
device->vTable = NULL;
motorCount = 0;
return;
}
int pinIndex = IO_GPIOPinIdx(io);
@ -753,8 +747,6 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
IOHi(io);
}
}
return &bbDevice;
}
#endif // USE_DSHOT_BB

59
src/platform/AT32/pwm_output_at32bsp.c
View file

@ -36,6 +36,7 @@
#include "pg/motor.h"
FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static int motorCount = 0;
static void pwmOCConfig(tmr_type *tim, uint8_t channel, uint16_t value, uint8_t output)
{
@ -76,13 +77,7 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
*channel->ccr = 0;
}
static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice;
static void pwmWriteUnused(uint8_t index, float value)
{
UNUSED(index);
UNUSED(value);
}
static FAST_DATA_ZERO_INIT motorDevice_t *motorPwmDevice;
static void pwmWriteStandard(uint8_t index, float value)
{
@ -92,7 +87,7 @@ static void pwmWriteStandard(uint8_t index, float value)
void pwmShutdownPulsesForAllMotors(void)
{
for (int index = 0; index < motorPwmDevice.count; index++) {
for (int index = 0; index < motorCount; index++) {
// Set the compare register to 0, which stops the output pulsing if the timer overflows
if (motors[index].channel.ccr) {
*motors[index].channel.ccr = 0;
@ -109,7 +104,7 @@ static motorVTable_t motorPwmVTable;
bool pwmEnableMotors(void)
{
/* check motors can be enabled */
return (motorPwmVTable.write != &pwmWriteUnused);
return (motorPwmDevice->vTable);
}
bool pwmIsMotorEnabled(unsigned index)
@ -117,9 +112,15 @@ bool pwmIsMotorEnabled(unsigned index)
return motors[index].enabled;
}
static void pwmCompleteOneshotMotorUpdate(void)
static bool useContinuousUpdate = true;
static void pwmCompleteMotorUpdate(void)
{
for (int index = 0; index < motorPwmDevice.count; index++) {
if (useContinuousUpdate) {
return;
}
for (int index = 0; index < motorCount; index++) {
if (motors[index].forceOverflow) {
timerForceOverflow(motors[index].channel.tim);
}
@ -147,15 +148,25 @@ static motorVTable_t motorPwmVTable = {
.shutdown = pwmShutdownPulsesForAllMotors,
.convertExternalToMotor = pwmConvertFromExternal,
.convertMotorToExternal = pwmConvertToExternal,
.write = pwmWriteStandard,
.decodeTelemetry = motorDecodeTelemetryNull,
.updateComplete = pwmCompleteMotorUpdate,
.requestTelemetry = NULL,
.isMotorIdle = NULL,
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)
{
memset(motors, 0, sizeof(motors));
motorPwmDevice.vTable = motorPwmVTable;
if (!device) {
return;
}
device->vTable = &motorPwmVTable;
motorPwmDevice = device;
motorCount = device->count;
useContinuousUpdate = motorConfig->useUnsyncedUpdate;
float sMin = 0;
float sLen = 0;
@ -175,21 +186,17 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
break;
case MOTOR_PROTOCOL_BRUSHED:
sMin = 0;
useUnsyncedUpdate = true;
useContinuousUpdate = true;
idlePulse = 0;
break;
case MOTOR_PROTOCOL_PWM50HZ :
case MOTOR_PROTOCOL_PWM :
sMin = 1e-3f;
sLen = 1e-3f;
useUnsyncedUpdate = true;
useContinuousUpdate = true;
idlePulse = 0;
break;
}
motorPwmDevice.vTable.write = pwmWriteStandard;
motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
@ -197,10 +204,10 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
motorPwmDevice.vTable.write = &pwmWriteUnused;
motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
device->vTable = NULL;
motorCount = 0;
/* TODO: block arming and add reason system cannot arm */
return NULL;
return;
}
motors[motorIndex].io = IOGetByTag(tag);
@ -210,13 +217,13 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
/* standard PWM outputs */
// margin of safety is 4 periods when unsynced
const unsigned pwmRateHz = useUnsyncedUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
const uint32_t clock = timerClock(timerHardware->tim);
/* used to find the desired timer frequency for max resolution */
const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
const uint32_t hz = clock / prescaler;
const unsigned period = useUnsyncedUpdate ? hz / pwmRateHz : 0xffff;
const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff;
/*
if brushed then it is the entire length of the period.
@ -238,8 +245,6 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
motors[motorIndex].forceOverflow = !timerAlreadyUsed;
motors[motorIndex].enabled = true;
}
return &motorPwmDevice;
}
pwmOutputPort_t *pwmGetMotors(void)

2
src/platform/AT32/pwm_output_dshot.c
View file

@ -249,7 +249,7 @@ void pwmCompleteDshotMotorUpdate(void)
{
// If there is a dshot command loaded up, time it correctly with motor update
if (!dshotCommandQueueEmpty()) {
if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
if (!dshotCommandOutputIsEnabled(motorCount)) {
return;
}
}

18
src/platform/SIMULATOR/sitl.c
View file

@ -611,8 +611,7 @@ void pwmWriteServo(uint8_t index, float value)
}
}
static motorDevice_t motorPwmDevice = {
.vTable = {
static const motorVTable_t vTable = {
.postInit = motorPostInitNull,
.convertExternalToMotor = pwmConvertFromExternal,
.convertMotorToExternal = pwmConvertToExternal,
@ -626,14 +625,18 @@ static motorDevice_t motorPwmDevice = {
.shutdown = pwmShutdownPulsesForAllMotors,
.requestTelemetry = NULL,
.isMotorIdle = NULL,
}
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t _idlePulse)
{
UNUSED(motorConfig);
UNUSED(useUnsyncedUpdate);
if (!device) {
return;
}
device->vTable = &vTable;
uint8_t motorCount = device->count;
printf("Initialized motor count %d\n", motorCount);
pwmRawPkt.motorCount = motorCount;
@ -642,11 +645,6 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t _id
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
motors[motorIndex].enabled = true;
}
motorPwmDevice.count = motorCount; // Never used, but seemingly a right thing to set it anyways.
motorPwmDevice.initialized = true;
motorPwmDevice.enabled = false;
return &motorPwmDevice;
}
// ADC part

29
src/platform/STM32/dshot_bitbang.c
View file

@ -64,7 +64,6 @@ FAST_DATA_ZERO_INIT int usedMotorPorts;
FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS];
static FAST_DATA_ZERO_INIT int motorCount;
dshotBitbangStatus_e bbStatus;
// For MCUs that use MPU to control DMA coherency, there might be a performance hit
@ -133,7 +132,6 @@ const timerHardware_t bbTimerHardware[] = {
#endif
};
static FAST_DATA_ZERO_INIT motorDevice_t bbDevice;
static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
static motorProtocolTypes_e motorProtocol;
@ -457,10 +455,6 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmP
}
if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
return false;
}
@ -641,7 +635,7 @@ static void bbUpdateComplete(void)
// If there is a dshot command loaded up, time it correctly with motor update
if (!dshotCommandQueueEmpty()) {
if (!dshotCommandOutputIsEnabled(bbDevice.count)) {
if (!dshotCommandOutputIsEnabled(motorCount)) {
return;
}
}
@ -716,7 +710,7 @@ static void bbPostInit(void)
}
}
static motorVTable_t bbVTable = {
static const motorVTable_t bbVTable = {
.postInit = bbPostInit,
.enable = bbEnableMotors,
.disable = bbDisableMotors,
@ -739,14 +733,19 @@ dshotBitbangStatus_e dshotBitbangGetStatus(void)
return bbStatus;
}
motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count)
void dshotBitbangDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)
{
dbgPinLo(0);
dbgPinLo(1);
if (!device || !motorConfig) {
return;
}
motorProtocol = motorConfig->motorProtocol;
bbDevice.vTable = bbVTable;
motorCount = count;
device->vTable = &bbVTable;
motorCount = device->count;
bbStatus = DSHOT_BITBANG_STATUS_OK;
#ifdef USE_DSHOT_TELEMETRY
@ -771,11 +770,10 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
if (!IOIsFreeOrPreinit(io)) {
/* not enough motors initialised for the mixer or a break in the motors */
bbDevice.vTable.write = motorWriteNull;
bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
device->vTable = NULL;
motorCount = 0;
bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
return NULL;
return;
}
int pinIndex = IO_GPIOPinIdx(io);
@ -798,7 +796,6 @@ motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t
}
}
return &bbDevice;
}
#endif // USE_DSHOT_BB

61
src/platform/STM32/pwm_output.c
View file

@ -28,15 +28,17 @@
#ifdef USE_PWM_OUTPUT
#include "drivers/io.h"
#include "drivers/motor.h"
#include "drivers/pwm_output.h"
#include "drivers/time.h"
#include "drivers/timer.h"
#include "pg/motor.h"
#include "drivers/motor_impl.h"
FAST_DATA_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
static int motorCount = 0;
static bool useContinuousUpdate = true;
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
{
#if defined(USE_HAL_DRIVER)
@ -108,14 +110,6 @@ void pwmOutConfig(timerChannel_t *channel, const timerHardware_t *timerHardware,
*channel->ccr = 0;
}
static FAST_DATA_ZERO_INIT motorDevice_t motorPwmDevice;
static void pwmWriteUnused(uint8_t index, float value)
{
UNUSED(index);
UNUSED(value);
}
static void pwmWriteStandard(uint8_t index, float value)
{
/* TODO: move value to be a number between 0-1 (i.e. percent throttle from mixer) */
@ -124,7 +118,7 @@ static void pwmWriteStandard(uint8_t index, float value)
void pwmShutdownPulsesForAllMotors(void)
{
for (int index = 0; index < motorPwmDevice.count; index++) {
for (int index = 0; motorCount; index++) {
// Set the compare register to 0, which stops the output pulsing if the timer overflows
if (motors[index].channel.ccr) {
*motors[index].channel.ccr = 0;
@ -137,11 +131,10 @@ void pwmDisableMotors(void)
pwmShutdownPulsesForAllMotors();
}
static motorVTable_t motorPwmVTable;
bool pwmEnableMotors(void)
{
/* check motors can be enabled */
return (motorPwmVTable.write != &pwmWriteUnused);
return motorCount > 0;
}
bool pwmIsMotorEnabled(unsigned index)
@ -149,9 +142,13 @@ bool pwmIsMotorEnabled(unsigned index)
return motors[index].enabled;
}
static void pwmCompleteOneshotMotorUpdate(void)
static void pwmCompleteMotorUpdate(void)
{
for (int index = 0; index < motorPwmDevice.count; index++) {
if (useContinuousUpdate) {
return;
}
for (int index = 0; motorCount; index++) {
if (motors[index].forceOverflow) {
timerForceOverflow(motors[index].channel.tim);
}
@ -171,23 +168,28 @@ static uint16_t pwmConvertToExternal(float motorValue)
return (uint16_t)motorValue;
}
static motorVTable_t motorPwmVTable = {
.postInit = motorPostInitNull,
static const motorVTable_t motorPwmVTable = {
.postInit = NULL,
.enable = pwmEnableMotors,
.disable = pwmDisableMotors,
.isMotorEnabled = pwmIsMotorEnabled,
.shutdown = pwmShutdownPulsesForAllMotors,
.convertExternalToMotor = pwmConvertFromExternal,
.convertMotorToExternal = pwmConvertToExternal,
.write = pwmWriteStandard,
.decodeTelemetry = NULL,
.updateComplete = pwmCompleteMotorUpdate,
.requestTelemetry = NULL,
.isMotorIdle = NULL,
};
motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
void motorPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig, uint16_t idlePulse)
{
memset(motors, 0, sizeof(motors));
motorPwmDevice.vTable = motorPwmVTable;
motorCount = device->count;
device->vTable = &motorPwmVTable;
useContinuousUpdate = motorConfig->useUnsyncedUpdate;
float sMin = 0;
float sLen = 0;
@ -207,21 +209,17 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
break;
case MOTOR_PROTOCOL_BRUSHED:
sMin = 0;
useUnsyncedUpdate = true;
useContinuousUpdate = true;
idlePulse = 0;
break;
case MOTOR_PROTOCOL_PWM50HZ :
case MOTOR_PROTOCOL_PWM :
sMin = 1e-3f;
sLen = 1e-3f;
useUnsyncedUpdate = true;
useContinuousUpdate = true;
idlePulse = 0;
break;
}
motorPwmDevice.vTable.write = pwmWriteStandard;
motorPwmDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
motorPwmDevice.vTable.updateComplete = useUnsyncedUpdate ? motorUpdateCompleteNull : pwmCompleteOneshotMotorUpdate;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
@ -229,10 +227,10 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
if (timerHardware == NULL) {
/* not enough motors initialised for the mixer or a break in the motors */
motorPwmDevice.vTable.write = &pwmWriteUnused;
motorPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
device->vTable = NULL;
motorCount = 0;
/* TODO: block arming and add reason system cannot arm */
return NULL;
return;
}
motors[motorIndex].io = IOGetByTag(tag);
@ -242,13 +240,13 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
/* standard PWM outputs */
// margin of safety is 4 periods when unsynced
const unsigned pwmRateHz = useUnsyncedUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
const unsigned pwmRateHz = useContinuousUpdate ? motorConfig->motorPwmRate : ceilf(1 / ((sMin + sLen) * 4));
const uint32_t clock = timerClock(timerHardware->tim);
/* used to find the desired timer frequency for max resolution */
const unsigned prescaler = ((clock / pwmRateHz) + 0xffff) / 0x10000; /* rounding up */
const uint32_t hz = clock / prescaler;
const unsigned period = useUnsyncedUpdate ? hz / pwmRateHz : 0xffff;
const unsigned period = useContinuousUpdate ? hz / pwmRateHz : 0xffff;
/*
if brushed then it is the entire length of the period.
@ -271,7 +269,6 @@ motorDevice_t *motorPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
motors[motorIndex].enabled = true;
}
return &motorPwmDevice;
}
pwmOutputPort_t *pwmGetMotors(void)

2
src/platform/STM32/pwm_output_dshot.c
View file

@ -144,7 +144,7 @@ void pwmCompleteDshotMotorUpdate(void)
{
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (!dshotCommandQueueEmpty()) {
if (!dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
if (!dshotCommandOutputIsEnabled(motorCount)) {
return;
}
}

2
src/platform/STM32/pwm_output_dshot_hal.c
View file

@ -138,7 +138,7 @@ FAST_CODE static void pwmDshotSetDirectionInput(
FAST_CODE void pwmCompleteDshotMotorUpdate(void)
{
/* If there is a dshot command loaded up, time it correctly with motor update*/
if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(dshotPwmDevice.count)) {
if (!dshotCommandQueueEmpty() && !dshotCommandOutputIsEnabled(motorCount)) {
return;
}

6
src/platform/common/stm32/dshot_bitbang_impl.h
View file

@ -20,8 +20,12 @@
#pragma once
#include "platform.h"
#include "common/time.h"
#include "drivers/motor.h"
#include "drivers/timer.h"
#include "drivers/motor_types.h"
#include "drivers/dshot.h"
#define USE_DMA_REGISTER_CACHE

2
src/platform/common/stm32/dshot_bitbang_shared.c
View file

@ -41,5 +41,5 @@ bool bbDshotIsMotorIdle(unsigned motorIndex)
}
bbMotor_t *const bbmotor = &bbMotors[motorIndex];
return bbmotor->protocolControl.value != 0;
return bbmotor->protocolControl.value == 0;
}

27
src/platform/common/stm32/dshot_dpwm.c
View file

@ -111,7 +111,7 @@ static void dshotPwmDisableMotors(void)
static bool dshotPwmEnableMotors(void)
{
for (int i = 0; i < dshotPwmDevice.count; i++) {
for (int i = 0; i < motorCount; i++) {
motorDmaOutput_t *motor = getMotorDmaOutput(i);
const IO_t motorIO = IOGetByTag(motor->timerHardware->tag);
IOConfigGPIOAF(motorIO, motor->iocfg, motor->timerHardware->alternateFunction);
@ -136,12 +136,12 @@ static FAST_CODE void dshotWrite(uint8_t index, float value)
pwmWriteDshotInt(index, lrintf(value));
}
static motorVTable_t dshotPwmVTable = {
static const motorVTable_t dshotPwmVTable = {
.postInit = motorPostInitNull,
.enable = dshotPwmEnableMotors,
.disable = dshotPwmDisableMotors,
.isMotorEnabled = dshotPwmIsMotorEnabled,
.decodeTelemetry = motorDecodeTelemetryNull, // May be updated after copying
.decodeTelemetry = pwmTelemetryDecode,
.write = dshotWrite,
.writeInt = dshotWriteInt,
.updateComplete = pwmCompleteDshotMotorUpdate,
@ -152,18 +152,12 @@ static motorVTable_t dshotPwmVTable = {
.isMotorIdle = pwmDshotIsMotorIdle,
};
FAST_DATA_ZERO_INIT motorDevice_t dshotPwmDevice;
motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedUpdate)
void dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)
{
UNUSED(idlePulse);
UNUSED(useUnsyncedUpdate);
dshotPwmDevice.vTable = dshotPwmVTable;
device->vTable = &dshotPwmVTable;
motorCount = device->count;
#ifdef USE_DSHOT_TELEMETRY
useDshotTelemetry = motorConfig->useDshotTelemetry;
dshotPwmDevice.vTable.decodeTelemetry = pwmTelemetryDecode;
#endif
switch (motorConfig->motorProtocol) {
@ -202,14 +196,11 @@ motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idl
}
/* not enough motors initialised for the mixer or a break in the motors */
dshotPwmDevice.vTable.write = motorWriteNull;
dshotPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
device->vTable = NULL;
motorCount = 0;
/* TODO: block arming and add reason system cannot arm */
return NULL;
return;
}
return &dshotPwmDevice;
}
#endif // USE_DSHOT

2
src/platform/common/stm32/dshot_dpwm.h
View file

@ -163,5 +163,3 @@ bool pwmTelemetryDecode(void);
void pwmCompleteDshotMotorUpdate(void);
extern bool useBurstDshot;
extern motorDevice_t dshotPwmDevice;

14
src/platform/common/stm32/pwm_output_dshot_shared.c
View file

@ -49,6 +49,8 @@
#include "pwm_output_dshot_shared.h"
FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount = 0;
FAST_DATA_ZERO_INIT uint8_t motorCount = 0;
#ifdef STM32F7
FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
@ -74,7 +76,7 @@ motorDmaOutput_t *getMotorDmaOutput(unsigned index)
bool pwmDshotIsMotorIdle(unsigned index)
{
const motorDmaOutput_t *motor = getMotorDmaOutput(index);
return motor && motor->protocolControl.value != 0;
return motor && motor->protocolControl.value == 0;
}
void pwmDshotRequestTelemetry(unsigned index)
@ -208,13 +210,13 @@ static uint32_t decodeTelemetryPacket(const uint32_t buffer[], uint32_t count)
#endif
#ifdef USE_DSHOT_TELEMETRY
/**
* Process dshot telemetry packets before switching the channels back to outputs
*
*/
FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)
{
#ifdef USE_DSHOT_TELEMETRY
if (!useDshotTelemetry) {
return true;
}
@ -224,7 +226,7 @@ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)
#endif
const timeUs_t currentUs = micros();
for (int i = 0; i < dshotPwmDevice.count; i++) {
for (int i = 0; i < motorCount; i++) {
timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs);
if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
return false;
@ -274,9 +276,11 @@ FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)
dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
inputStampUs = 0;
dshotEnableChannels(dshotPwmDevice.count);
dshotEnableChannels(motorCount);
#endif // USE_DSHOT_TELEMETRY
return true;
}
#endif // USE_DSHOT_TELEMETRY
#endif // USE_DSHOT

1
src/platform/common/stm32/pwm_output_dshot_shared.h
View file

@ -24,6 +24,7 @@
#include "dshot_dpwm.h"
extern FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount;
extern FAST_DATA_ZERO_INIT uint8_t motorCount;
extern FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
extern FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];