diff --git a/src/main/config/config.c b/src/main/config/config.c index ccaca2eb17..58c9d146d9 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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); } diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h index f87bc11a15..3896cfbd6f 100644 --- a/src/main/drivers/dshot.h +++ b/src/main/drivers/dshot.h @@ -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; diff --git a/src/main/drivers/dshot_bitbang.h b/src/main/drivers/dshot_bitbang.h index b24a8f446b..a9aacd2d79 100644 --- a/src/main/drivers/dshot_bitbang.h +++ b/src/main/drivers/dshot_bitbang.h @@ -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); diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c index 33c29282ca..90f80185d8 100644 --- a/src/main/drivers/dshot_command.c +++ b/src/main/drivers/dshot_command.c @@ -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); diff --git a/src/main/drivers/dshot_command.h b/src/main/drivers/dshot_command.h index 9ade0d459f..aa9b0d2b16 100644 --- a/src/main/drivers/dshot_command.h +++ b/src/main/drivers/dshot_command.h @@ -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); diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c index 82479dc558..a9c1a6a427 100644 --- a/src/main/drivers/motor.c +++ b/src/main/drivers/motor.c @@ -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 diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h index 3c451882fe..32cadfea90 100644 --- a/src/main/drivers/motor.h +++ b/src/main/drivers/motor.h @@ -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 diff --git a/src/main/drivers/motor_impl.h b/src/main/drivers/motor_impl.h new file mode 100644 index 0000000000..02d9f226fd --- /dev/null +++ b/src/main/drivers/motor_impl.h @@ -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 . + */ + +#pragma once + +#include "drivers/motor_types.h" + +void motorPostInitNull(void); +bool motorDecodeTelemetryNull(void); +void motorUpdateCompleteNull(void); + +void motorNullDevInit(motorDevice_t *device); diff --git a/src/main/drivers/motor_types.h b/src/main/drivers/motor_types.h index 33cd100b8f..5496ab2af8 100644 --- a/src/main/drivers/motor_types.h +++ b/src/main/drivers/motor_types.h @@ -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; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 210140105e..7a98738e10 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -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) diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 84abb8f4e1..14d46ca92f 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -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) {} diff --git a/src/main/pg/motor.c b/src/main/pg/motor.c index 80f75c98c7..2c02160014 100644 --- a/src/main/pg/motor.c +++ b/src/main/pg/motor.c @@ -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; diff --git a/src/main/pg/motor.h b/src/main/pg/motor.h index e829b15472..f44e228a0e 100644 --- a/src/main/pg/motor.h +++ b/src/main/pg/motor.h @@ -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, diff --git a/src/platform/APM32/dshot_bitbang.c b/src/platform/APM32/dshot_bitbang.c index bac6caff8b..52c3d8b43f 100644 --- a/src/platform/APM32/dshot_bitbang.c +++ b/src/platform/APM32/dshot_bitbang.c @@ -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 diff --git a/src/platform/APM32/pwm_output_apm32.c b/src/platform/APM32/pwm_output_apm32.c index 5250877923..cd20576752 100644 --- a/src/platform/APM32/pwm_output_apm32.c +++ b/src/platform/APM32/pwm_output_apm32.c @@ -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) diff --git a/src/platform/APM32/pwm_output_dshot_apm32.c b/src/platform/APM32/pwm_output_dshot_apm32.c index 64738d1b97..33118007ec 100644 --- a/src/platform/APM32/pwm_output_dshot_apm32.c +++ b/src/platform/APM32/pwm_output_dshot_apm32.c @@ -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; } diff --git a/src/platform/AT32/dshot_bitbang.c b/src/platform/AT32/dshot_bitbang.c index 20eb05cd22..50cfeea854 100644 --- a/src/platform/AT32/dshot_bitbang.c +++ b/src/platform/AT32/dshot_bitbang.c @@ -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 diff --git a/src/platform/AT32/pwm_output_at32bsp.c b/src/platform/AT32/pwm_output_at32bsp.c index 62da81f576..01a975d309 100644 --- a/src/platform/AT32/pwm_output_at32bsp.c +++ b/src/platform/AT32/pwm_output_at32bsp.c @@ -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) diff --git a/src/platform/AT32/pwm_output_dshot.c b/src/platform/AT32/pwm_output_dshot.c index 892a995d6a..10686c8c03 100644 --- a/src/platform/AT32/pwm_output_dshot.c +++ b/src/platform/AT32/pwm_output_dshot.c @@ -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; } } diff --git a/src/platform/SIMULATOR/sitl.c b/src/platform/SIMULATOR/sitl.c index 29a5dee8dd..14ceca0310 100644 --- a/src/platform/SIMULATOR/sitl.c +++ b/src/platform/SIMULATOR/sitl.c @@ -611,29 +611,32 @@ void pwmWriteServo(uint8_t index, float value) } } -static motorDevice_t motorPwmDevice = { - .vTable = { - .postInit = motorPostInitNull, - .convertExternalToMotor = pwmConvertFromExternal, - .convertMotorToExternal = pwmConvertToExternal, - .enable = pwmEnableMotors, - .disable = pwmDisableMotors, - .isMotorEnabled = pwmIsMotorEnabled, - .decodeTelemetry = motorDecodeTelemetryNull, - .write = pwmWriteMotor, - .writeInt = pwmWriteMotorInt, - .updateComplete = pwmCompleteMotorUpdate, - .shutdown = pwmShutdownPulsesForAllMotors, - .requestTelemetry = NULL, - .isMotorIdle = NULL, - } +static const motorVTable_t vTable = { + .postInit = motorPostInitNull, + .convertExternalToMotor = pwmConvertFromExternal, + .convertMotorToExternal = pwmConvertToExternal, + .enable = pwmEnableMotors, + .disable = pwmDisableMotors, + .isMotorEnabled = pwmIsMotorEnabled, + .decodeTelemetry = motorDecodeTelemetryNull, + .write = pwmWriteMotor, + .writeInt = pwmWriteMotorInt, + .updateComplete = pwmCompleteMotorUpdate, + .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 diff --git a/src/platform/STM32/dshot_bitbang.c b/src/platform/STM32/dshot_bitbang.c index 0990848520..cbaeff1135 100644 --- a/src/platform/STM32/dshot_bitbang.c +++ b/src/platform/STM32/dshot_bitbang.c @@ -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 diff --git a/src/platform/STM32/pwm_output.c b/src/platform/STM32/pwm_output.c index bea0a9b2bb..e25e01ec4c 100644 --- a/src/platform/STM32/pwm_output.c +++ b/src/platform/STM32/pwm_output.c @@ -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) diff --git a/src/platform/STM32/pwm_output_dshot.c b/src/platform/STM32/pwm_output_dshot.c index bddbba54b4..e989b1e738 100644 --- a/src/platform/STM32/pwm_output_dshot.c +++ b/src/platform/STM32/pwm_output_dshot.c @@ -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; } } diff --git a/src/platform/STM32/pwm_output_dshot_hal.c b/src/platform/STM32/pwm_output_dshot_hal.c index 2d313d778c..4e2589c698 100644 --- a/src/platform/STM32/pwm_output_dshot_hal.c +++ b/src/platform/STM32/pwm_output_dshot_hal.c @@ -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; } diff --git a/src/platform/common/stm32/dshot_bitbang_impl.h b/src/platform/common/stm32/dshot_bitbang_impl.h index ab23eeb0ac..ab93c8a5bd 100644 --- a/src/platform/common/stm32/dshot_bitbang_impl.h +++ b/src/platform/common/stm32/dshot_bitbang_impl.h @@ -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 diff --git a/src/platform/common/stm32/dshot_bitbang_shared.c b/src/platform/common/stm32/dshot_bitbang_shared.c index 2820d2307a..15f9d0572b 100644 --- a/src/platform/common/stm32/dshot_bitbang_shared.c +++ b/src/platform/common/stm32/dshot_bitbang_shared.c @@ -41,5 +41,5 @@ bool bbDshotIsMotorIdle(unsigned motorIndex) } bbMotor_t *const bbmotor = &bbMotors[motorIndex]; - return bbmotor->protocolControl.value != 0; + return bbmotor->protocolControl.value == 0; } diff --git a/src/platform/common/stm32/dshot_dpwm.c b/src/platform/common/stm32/dshot_dpwm.c index 8376680b23..aa10d1869e 100644 --- a/src/platform/common/stm32/dshot_dpwm.c +++ b/src/platform/common/stm32/dshot_dpwm.c @@ -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 diff --git a/src/platform/common/stm32/dshot_dpwm.h b/src/platform/common/stm32/dshot_dpwm.h index 63a403bd4e..0cdf3cf41b 100644 --- a/src/platform/common/stm32/dshot_dpwm.h +++ b/src/platform/common/stm32/dshot_dpwm.h @@ -163,5 +163,3 @@ bool pwmTelemetryDecode(void); void pwmCompleteDshotMotorUpdate(void); extern bool useBurstDshot; - -extern motorDevice_t dshotPwmDevice; diff --git a/src/platform/common/stm32/pwm_output_dshot_shared.c b/src/platform/common/stm32/pwm_output_dshot_shared.c index b49edb0f3a..21de80a11b 100644 --- a/src/platform/common/stm32/pwm_output_dshot_shared.c +++ b/src/platform/common/stm32/pwm_output_dshot_shared.c @@ -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 diff --git a/src/platform/common/stm32/pwm_output_dshot_shared.h b/src/platform/common/stm32/pwm_output_dshot_shared.h index 38f226b305..dbcb2b8fb1 100644 --- a/src/platform/common/stm32/pwm_output_dshot_shared.h +++ b/src/platform/common/stm32/pwm_output_dshot_shared.h @@ -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];