From d49948f1fdde9c8ce97015a5836ba7fa06d50166 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 16 Jun 2018 12:23:14 +1200 Subject: [PATCH 1/3] Stop Dshot commands from running when ESCs not disarmed. --- src/main/drivers/pwm_output.c | 65 +++++++++++++++++-------- src/main/drivers/pwm_output.h | 8 +-- src/main/drivers/pwm_output_dshot.c | 16 +++--- src/main/drivers/pwm_output_dshot_hal.c | 14 ++++-- 4 files changed, 69 insertions(+), 34 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 7e7c89a172..d8359ae540 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -42,13 +42,14 @@ FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; #define DSHOT_BEEP_DELAY_US 100000 typedef struct dshotCommandControl_s { - timeUs_t nextCommandAt; - timeUs_t delayAfterCommand; + timeUs_t nextCommandAt; + timeUs_t delayAfterCommand; + bool waitingForIdle; + uint8_t repeats; uint8_t command[MAX_SUPPORTED_MOTORS]; - uint8_t repeats; } dshotCommandControl_t; -dshotCommandControl_t dshotCommandControl; +static dshotCommandControl_t dshotCommandControl; #endif #ifdef USE_SERVOS @@ -386,16 +387,21 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } } -FAST_CODE bool pwmIsProcessingDshotCommand(void) +FAST_CODE bool pwmDshotCommandIsQueued(void) { return dshotCommandControl.nextCommandAt; } +FAST_CODE bool pwmDshotCommandIsProcessing(void) +{ + return dshotCommandControl.nextCommandAt && !dshotCommandControl.waitingForIdle; +} + void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking) { timeUs_t timeNowUs = micros(); - if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND)) { + if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || pwmDshotCommandIsQueued()) { return; } @@ -427,8 +433,9 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo if (blocking) { for (; repeats; repeats--) { + delayMicroseconds(DSHOT_COMMAND_DELAY_US); + for (uint8_t i = 0; i < motorCount; i++) { - delayMicroseconds(DSHOT_COMMAND_DELAY_US); if ((i == index) || (index == ALL_MOTORS)) { motorDmaOutput_t *const motor = getMotorDmaOutput(i); motor->requestTelemetry = true; @@ -440,14 +447,13 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo } delayMicroseconds(timeDelayUs); } else { - if (!pwmIsProcessingDshotCommand()) { - for (uint8_t i = 0; i < motorCount; i++) { - if ((index == i) || (index == ALL_MOTORS)) { - dshotCommandControl.command[i] = command; - dshotCommandControl.repeats = repeats; - dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US; - dshotCommandControl.delayAfterCommand = timeDelayUs; - } + dshotCommandControl.repeats = repeats; + dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US; + dshotCommandControl.delayAfterCommand = timeDelayUs; + dshotCommandControl.waitingForIdle = true; + for (unsigned i = 0; i < motorCount; i++) { + if (index == i || index == ALL_MOTORS) { + dshotCommandControl.command[i] = command; } } } @@ -458,11 +464,30 @@ uint8_t pwmGetDshotCommand(uint8_t index) return dshotCommandControl.command[index]; } -FAST_CODE_NOINLINE bool pwmProcessDshotCommand(uint8_t motorCount) +FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount) { timeUs_t timeNowUs = micros(); + + if (dshotCommandControl.waitingForIdle) { + bool allMotorsIdle = true; + for (unsigned i = 0; i < motorCount; i++) { + const motorDmaOutput_t *motor = getMotorDmaOutput(i); + if (motor->value) { + allMotorsIdle = false; + } + } + + if (allMotorsIdle) { + dshotCommandControl.waitingForIdle = false; + } + + // Send normal motor output while waiting for motors to go idle + return true; + } + if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAt) < 0) { - return false; //Skip motor update because it isn't time yet for a new command + //Skip motor update because it isn't time yet for a new command + return false; } //Timed motor update happening with dshot command @@ -473,7 +498,7 @@ FAST_CODE_NOINLINE bool pwmProcessDshotCommand(uint8_t motorCount) dshotCommandControl.nextCommandAt = timeNowUs + dshotCommandControl.delayAfterCommand; } } else { - for (uint8_t i = 0; i < motorCount; i++) { + for (unsigned i = 0; i < motorCount; i++) { dshotCommandControl.command[i] = 0; } dshotCommandControl.nextCommandAt = 0; @@ -483,9 +508,9 @@ FAST_CODE_NOINLINE bool pwmProcessDshotCommand(uint8_t motorCount) return true; } -FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, const uint16_t value) +FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor) { - uint16_t packet = (value << 1) | (motor->requestTelemetry ? 1 : 0); + uint16_t packet = (motor->value << 1) | (motor->requestTelemetry ? 1 : 0); motor->requestTelemetry = false; // reset telemetry request to make sure it's triggered only once in a row // compute checksum diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 74f1594632..a1dbe30ee9 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -191,7 +191,7 @@ bool isMotorProtocolDshot(void); #ifdef USE_DSHOT typedef uint8_t loadDmaBufferFn(uint32_t *dmaBuffer, int stride, uint16_t packet); // function pointer used to encode a digital motor value into the DMA buffer representation -uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value); +uint16_t prepareDshotPacket(motorDmaOutput_t *const motor); extern loadDmaBufferFn *loadDmaBuffer; @@ -201,9 +201,11 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo void pwmWriteDshotInt(uint8_t index, uint16_t value); void pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output); void pwmCompleteDshotMotorUpdate(uint8_t motorCount); -bool pwmIsProcessingDshotCommand(void); + +bool pwmDshotCommandIsQueued(void); +bool pwmDshotCommandIsProcessing(void); uint8_t pwmGetDshotCommand(uint8_t index); -bool pwmProcessDshotCommand(uint8_t motorCount); +bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount); #endif diff --git a/src/main/drivers/pwm_output_dshot.c b/src/main/drivers/pwm_output_dshot.c index 53e6b3b100..ee81dffb60 100644 --- a/src/main/drivers/pwm_output_dshot.c +++ b/src/main/drivers/pwm_output_dshot.c @@ -70,12 +70,16 @@ void pwmWriteDshotInt(uint8_t index, uint16_t value) } /*If there is a command ready to go overwrite the value and send that instead*/ - if (pwmIsProcessingDshotCommand()) { + if (pwmDshotCommandIsProcessing()) { value = pwmGetDshotCommand(index); - motor->requestTelemetry = true; + if (value) { + motor->requestTelemetry = true; + } } - - uint16_t packet = prepareDshotPacket(motor, value); + + motor->value = value; + + uint16_t packet = prepareDshotPacket(motor); uint8_t bufferSize; #ifdef USE_DSHOT_DMAR @@ -97,8 +101,8 @@ void pwmCompleteDshotMotorUpdate(uint8_t motorCount) UNUSED(motorCount); /* If there is a dshot command loaded up, time it correctly with motor update*/ - if (pwmIsProcessingDshotCommand()) { - if (!pwmProcessDshotCommand(motorCount)) { + if (pwmDshotCommandIsQueued()) { + if (!pwmDshotCommandOutputIsEnabled(motorCount)) { return; } } diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index e0ba048639..6c62baff06 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -63,12 +63,16 @@ FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value) } /*If there is a command ready to go overwrite the value and send that instead*/ - if (pwmIsProcessingDshotCommand()) { + if (pwmDshotCommandIsProcessing()) { value = pwmGetDshotCommand(index); - motor->requestTelemetry = true; + if (value) { + motor->requestTelemetry = true; + } } - uint16_t packet = prepareDshotPacket(motor, value); + motor->value = value; + + uint16_t packet = prepareDshotPacket(motor); uint8_t bufferSize; #ifdef USE_DSHOT_DMAR @@ -90,8 +94,8 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(uint8_t motorCount) UNUSED(motorCount); /* If there is a dshot command loaded up, time it correctly with motor update*/ - if (pwmIsProcessingDshotCommand()) { - if (!pwmProcessDshotCommand(motorCount)) { + if (pwmDshotCommandIsQueued()) { + if (!pwmDshotCommandOutputIsEnabled(motorCount)) { return; } } From 7f107e2d9bd62d7cc4619e8619cb91f120c1e048 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 17 Jun 2018 01:45:10 +1200 Subject: [PATCH 2/3] Added updating of scheduled command time when motors become idle. --- src/main/drivers/pwm_output.c | 71 ++++++++++++++++++++--------------- 1 file changed, 40 insertions(+), 31 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index d8359ae540..f13d164017 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -37,13 +37,14 @@ static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL; #ifdef USE_DSHOT FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; +#define DSHOT_INITIAL_DELAY_US 10000 #define DSHOT_COMMAND_DELAY_US 1000 -#define DSHOT_ESCINFO_DELAY_US 5000 +#define DSHOT_ESCINFO_DELAY_US 12000 #define DSHOT_BEEP_DELAY_US 100000 typedef struct dshotCommandControl_s { - timeUs_t nextCommandAt; - timeUs_t delayAfterCommand; + timeUs_t nextCommandAtUs; + timeUs_t delayAfterCommandUs; bool waitingForIdle; uint8_t repeats; uint8_t command[MAX_SUPPORTED_MOTORS]; @@ -387,14 +388,27 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } } +bool allMotorsAreIdle(uint8_t motorCount) +{ + bool allMotorsIdle = true; + for (unsigned i = 0; i < motorCount; i++) { + const motorDmaOutput_t *motor = getMotorDmaOutput(i); + if (motor->value) { + allMotorsIdle = false; + } + } + + return allMotorsIdle; +} + FAST_CODE bool pwmDshotCommandIsQueued(void) { - return dshotCommandControl.nextCommandAt; + return dshotCommandControl.nextCommandAtUs; } FAST_CODE bool pwmDshotCommandIsProcessing(void) { - return dshotCommandControl.nextCommandAt && !dshotCommandControl.waitingForIdle; + return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle; } void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking) @@ -406,7 +420,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo } uint8_t repeats = 1; - timeUs_t timeDelayUs = DSHOT_COMMAND_DELAY_US; + timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US; switch (command) { case DSHOT_CMD_SPIN_DIRECTION_1: @@ -417,7 +431,7 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: repeats = 10; - timeDelayUs = DSHOT_COMMAND_DELAY_US; + delayAfterCommandUs = DSHOT_COMMAND_DELAY_US; break; case DSHOT_CMD_BEACON1: case DSHOT_CMD_BEACON2: @@ -425,13 +439,14 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo case DSHOT_CMD_BEACON4: case DSHOT_CMD_BEACON5: repeats = 1; - timeDelayUs = DSHOT_BEEP_DELAY_US; + delayAfterCommandUs = DSHOT_BEEP_DELAY_US; break; default: break; } if (blocking) { + delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US); for (; repeats; repeats--) { delayMicroseconds(DSHOT_COMMAND_DELAY_US); @@ -445,17 +460,20 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo pwmCompleteDshotMotorUpdate(0); } - delayMicroseconds(timeDelayUs); + delayMicroseconds(delayAfterCommandUs); } else { dshotCommandControl.repeats = repeats; - dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US; - dshotCommandControl.delayAfterCommand = timeDelayUs; - dshotCommandControl.waitingForIdle = true; + 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.waitingForIdle = !allMotorsAreIdle(motorCount); } } @@ -469,15 +487,8 @@ FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount) timeUs_t timeNowUs = micros(); if (dshotCommandControl.waitingForIdle) { - bool allMotorsIdle = true; - for (unsigned i = 0; i < motorCount; i++) { - const motorDmaOutput_t *motor = getMotorDmaOutput(i); - if (motor->value) { - allMotorsIdle = false; - } - } - - if (allMotorsIdle) { + if (allMotorsAreIdle(motorCount)) { + dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_INITIAL_DELAY_US; dshotCommandControl.waitingForIdle = false; } @@ -485,24 +496,22 @@ FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount) return true; } - if (cmpTimeUs(timeNowUs, dshotCommandControl.nextCommandAt) < 0) { + if (cmpTimeUs(timeNowUs, dshotCommandControl.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) { + if (dshotCommandControl.repeats > 1) { + dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US; + } else { + dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs; + } + dshotCommandControl.repeats--; - dshotCommandControl.nextCommandAt = timeNowUs + DSHOT_COMMAND_DELAY_US; - if (dshotCommandControl.repeats == 0) { - dshotCommandControl.nextCommandAt = timeNowUs + dshotCommandControl.delayAfterCommand; - } } else { - for (unsigned i = 0; i < motorCount; i++) { - dshotCommandControl.command[i] = 0; - } - dshotCommandControl.nextCommandAt = 0; - dshotCommandControl.delayAfterCommand = 0; + dshotCommandControl.nextCommandAtUs = 0; } return true; From f0673248448ca3fda5a60bd81226d31b675b6773 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 17 Jun 2018 03:07:10 +1200 Subject: [PATCH 3/3] Fixed extra repeat. --- src/main/drivers/pwm_output.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index f13d164017..61b5a6425c 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -408,7 +408,7 @@ FAST_CODE bool pwmDshotCommandIsQueued(void) FAST_CODE bool pwmDshotCommandIsProcessing(void) { - return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle; + return dshotCommandControl.nextCommandAtUs && !dshotCommandControl.waitingForIdle && dshotCommandControl.repeats > 0; } void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking) @@ -431,14 +431,12 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command, bo case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: repeats = 10; - delayAfterCommandUs = DSHOT_COMMAND_DELAY_US; break; case DSHOT_CMD_BEACON1: case DSHOT_CMD_BEACON2: case DSHOT_CMD_BEACON3: case DSHOT_CMD_BEACON4: case DSHOT_CMD_BEACON5: - repeats = 1; delayAfterCommandUs = DSHOT_BEEP_DELAY_US; break; default: @@ -503,13 +501,13 @@ FAST_CODE_NOINLINE bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount) //Timed motor update happening with dshot command if (dshotCommandControl.repeats > 0) { - if (dshotCommandControl.repeats > 1) { + dshotCommandControl.repeats--; + + if (dshotCommandControl.repeats > 0) { dshotCommandControl.nextCommandAtUs = timeNowUs + DSHOT_COMMAND_DELAY_US; } else { dshotCommandControl.nextCommandAtUs = timeNowUs + dshotCommandControl.delayAfterCommandUs; } - - dshotCommandControl.repeats--; } else { dshotCommandControl.nextCommandAtUs = 0; }