From 7859e6d5407d71013af74ae9a31e543befb2d5ec Mon Sep 17 00:00:00 2001 From: Thorsten Laux Date: Sat, 12 Jan 2019 12:22:14 +0100 Subject: [PATCH] Add dshot command queue and limit max notch filter frequency --- src/main/drivers/pwm_output.c | 97 +++++++++++++++++-------- src/main/drivers/pwm_output.h | 1 + src/main/drivers/pwm_output_dshot.c | 1 + src/main/drivers/pwm_output_dshot_hal.c | 1 + src/main/fc/core.c | 17 +++-- src/main/fc/init.c | 12 +-- src/main/sensors/rpm_filter.c | 3 + src/main/target/common_post.h | 4 + 8 files changed, 93 insertions(+), 43 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 3d7edab960..d2f137675d 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -44,6 +44,7 @@ FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; #define DSHOT_COMMAND_DELAY_US 1000 #define DSHOT_ESCINFO_DELAY_US 12000 #define DSHOT_BEEP_DELAY_US 100000 +#define DSHOT_MAX_COMMANDS 3 typedef struct dshotCommandControl_s { timeUs_t nextCommandAtUs; @@ -53,7 +54,9 @@ typedef struct dshotCommandControl_s { uint8_t command[MAX_SUPPORTED_MOTORS]; } dshotCommandControl_t; -static dshotCommandControl_t dshotCommandControl; +static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1]; +static uint8_t commandQueueHead; +static uint8_t commandQueueTail; #endif #ifdef USE_SERVOS @@ -436,21 +439,53 @@ bool allMotorsAreIdle(uint8_t motorCount) return allMotorsIdle; } +FAST_CODE bool pwmDshotCommandQueueFull() +{ + return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail; +} + FAST_CODE bool pwmDshotCommandIsQueued(void) { - return dshotCommandControl.nextCommandAtUs; + return commandQueueHead != commandQueueTail; } FAST_CODE bool pwmDshotCommandIsProcessing(void) { - return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle && dshotCommandControl.repeats > 0; + if (!pwmDshotCommandIsQueued()) { + return false; + } + dshotCommandControl_t* command = &commandQueue[commandQueueTail]; + return command->nextCommandAtUs && !command->waitingForIdle && command->repeats > 0; } +FAST_CODE void pwmDshotCommandQueueUpdate(void) +{ + if (!pwmDshotCommandIsQueued()) { + return; + } + dshotCommandControl_t* command = &commandQueue[commandQueueTail]; + if (!command->nextCommandAtUs && !command->waitingForIdle && !command->repeats) { + commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1); + } +} + +static dshotCommandControl_t* addCommand() +{ + int newHead = (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1); + if (newHead == commandQueueTail) { + return NULL; + } + dshotCommandControl_t* control = &commandQueue[commandQueueHead]; + commandQueueHead = newHead; + return control; +} + + void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking) { timeUs_t timeNowUs = micros(); - if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandIsQueued()) { + if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandQueueFull()) { return; } @@ -500,56 +535,60 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo } delayMicroseconds(delayAfterCommandUs); } else { - dshotCommandControl.repeats = repeats; - dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; - dshotCommandControl.delayAfterCommandUs = delayAfterCommandUs; - for (unsigned i = 0; i < motorCount; i++) { - if (index == i || index == ALL_MOTORS) { - dshotCommandControl.command[i] = command; - } else { - dshotCommandControl.command[i] = command; + dshotCommandControl_t *commandControl = addCommand(); + if (commandControl) { + commandControl->repeats = repeats; + commandControl->nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; + commandControl->delayAfterCommandUs = delayAfterCommandUs; + for (unsigned i = 0; i < motorCount; i++) { + if (index == i || index == ALL_MOTORS) { + commandControl->command[i] = command; + } else { + commandControl->command[i] = command; + } } + commandControl->waitingForIdle = !allMotorsAreIdle(motorCount); } - - dshotCommandControl.waitingForIdle = !allMotorsAreIdle(motorCount); } } uint8_t pwmGetDshotCommand(uint8_t index) { - return dshotCommandControl.command[index]; + return commandQueue[commandQueueTail].command[index]; } FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount) { timeUs_t timeNowUs = micros(); - if (dshotCommandControl.waitingForIdle) { - if (allMotorsAreIdle(motorCount)) { - dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; - dshotCommandControl.waitingForIdle = false; + dshotCommandControl_t* command = &commandQueue[commandQueueTail]; + if (pwmDshotCommandIsQueued()) { + if (command->waitingForIdle) { + if (allMotorsAreIdle(motorCount)) { + command->nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; + command->waitingForIdle = false; + } + // Send normal motor output while waiting for motors to go idle + return true; } - - // Send normal motor output while waiting for motors to go idle - return true; } - if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAtUs) < 0) { + if (cmpTimeUs(timeNowUs, command->nextCommandAtUs) < 0) { //Skip motor update because it isn't time yet for a new command return false; } //Timed motor update happening with dshot command - if (dshotCommandControl.repeats > 0) { - dshotCommandControl.repeats--; + if (command->repeats > 0) { + command->repeats--; - if (dshotCommandControl.repeats > 0) { - dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US; + if (command->repeats > 0) { + command->nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US; } else { - dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs; + command->nextCommandAtUs = timeNowUs + command->delayAfterCommandUs; } } else { - dshotCommandControl.nextCommandAtUs = 0; + command->nextCommandAtUs = 0; } return true; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 74e9777a96..d9cb6fa168 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -237,6 +237,7 @@ void pwmStartDshotMotorUpdate(uint8_t motorCount); #endif void pwmCompleteDshotMotorUpdate(uint8_t motorCount); +void pwmDshotCommandQueueUpdate(void); bool pwmDshotCommandIsQueued(void); bool pwmDshotCommandIsProcessing(void); uint8_t pwmGetDshotCommand(uint8_t index); diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index b6819cc67f..883149eb58 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -324,6 +324,7 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount) dmaMotorTimers[i].timerDmaSources = 0; } } + pwmDshotCommandQueueUpdate(); } static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 8b2b2a3af1..52fa56cdfe 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -120,6 +120,7 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount) dmaMotorTimers[i].timerDmaSources = 0; } } + pwmDshotCommandQueueUpdate(); } static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 911c5ac15d..a6740b5039 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -382,14 +382,6 @@ void tryArm(void) const timeUs_t currentTimeUs = micros(); #ifdef USE_DSHOT -#ifdef USE_DSHOT_TELEMETRY - pwmWriteDshotCommand( - 255, getMotorCount(), - motorConfig()->dev.useDshotTelemetry ? - DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : - DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, true); -#endif - if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { if (tryingToArm == ARMING_DELAYED_DISARMED) { if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { @@ -404,6 +396,15 @@ void tryArm(void) } return; } + +#ifdef USE_DSHOT_TELEMETRY + if (isMotorProtocolDshot()) { + pwmWriteDshotCommand( + 255, getMotorCount(), motorConfig()->dev.useDshotTelemetry ? + DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, false); + } +#endif + if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) { flipOverAfterCrashActive = false; diff --git a/src/main/fc/init.c b/src/main/fc/init.c index c950af6ead..1f28fc4a68 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -214,10 +214,8 @@ void activateDshotTelemetry(struct dispatchEntry_s* self) if (!ARMING_FLAG(ARMED)) { pwmWriteDshotCommand( - 255, getMotorCount(), - motorConfig()->dev.useDshotTelemetry ? - DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : - DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, true); + 255, getMotorCount(), motorConfig()->dev.useDshotTelemetry ? + DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, false); } } @@ -772,8 +770,10 @@ void init(void) // TODO: potentially delete when feature is stable. Activation when arming is enough for flight. #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) - dispatchEnable(); - dispatchAdd(&activateDshotTelemetryEntry, 5000000); + if (motorConfig()->dev.useDshotTelemetry) { + dispatchEnable(); + dispatchAdd(&activateDshotTelemetryEntry, 5000000); + } #endif fcTasksInit(); diff --git a/src/main/sensors/rpm_filter.c b/src/main/sensors/rpm_filter.c index ab8a9e1d27..504494d796 100644 --- a/src/main/sensors/rpm_filter.c +++ b/src/main/sensors/rpm_filter.c @@ -177,6 +177,9 @@ void rpmFilterUpdate() frequency = currentFilter->minHz; } } + if (frequency > deactivateFreq) { + frequency = deactivateFreq; + } biquadFilter_t* template = ¤tFilter->notch[0][motor][harmonic]; // uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 2447cba406..349100e59d 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -247,3 +247,7 @@ #ifndef USE_CMS #undef USE_CMS_FAILSAFE_MENU #endif + +#ifndef USE_DSHOT_TELEMETRY +#undef USE_RPM_FILTER +#endif