From 91ffa4b88ca6ed0678e840afeaa634d68a339b89 Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 23 Aug 2017 01:52:49 +1200 Subject: [PATCH] Fixed Dshot command sending to all motors. --- src/main/drivers/pwm_output.c | 17 +++++++++++----- src/main/drivers/pwm_output.h | 4 +++- src/main/fc/cli.c | 38 ++++++++++++++++++----------------- src/main/fc/fc_core.c | 8 ++------ src/main/flight/mixer.h | 2 -- src/main/io/beeper.c | 4 +--- src/main/sensors/esc_sensor.c | 2 +- 7 files changed, 39 insertions(+), 36 deletions(-) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 62608d8ec4..067be78fc3 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -372,11 +372,9 @@ uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType) } } -void pwmWriteDshotCommand(uint8_t index, uint8_t command) +void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command) { if (isDshot && (command <= DSHOT_MAX_COMMAND)) { - motorDmaOutput_t *const motor = getMotorDmaOutput(index); - unsigned repeats; switch (command) { case DSHOT_CMD_SPIN_DIRECTION_1: @@ -394,8 +392,17 @@ void pwmWriteDshotCommand(uint8_t index, uint8_t command) } for (; repeats; repeats--) { - motor->requestTelemetry = true; - pwmWriteDshotInt(index, command); + for (uint8_t i = 0; i < motorCount; i++) { + if ((i == index) || (index == ALL_MOTORS)) { + motorDmaOutput_t *const motor = getMotorDmaOutput(i); + motor->requestTelemetry = true; + pwmWriteDshotInt(i, command); + } else { + // Needed to avoid DMA errors + pwmWriteDshotInt(i, DSHOT_CMD_MOTOR_STOP); + } + } + pwmCompleteDshotMotorUpdate(0); delay(1); } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index e53275f453..6e3e8c9065 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -24,6 +24,8 @@ #include "drivers/timer.h" +#define ALL_MOTORS 255 + #define DSHOT_MAX_COMMAND 47 /* @@ -168,7 +170,7 @@ uint16_t prepareDshotPacket(motorDmaOutput_t *const motor, uint16_t value); extern loadDmaBufferFn *loadDmaBuffer; uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType); -void pwmWriteDshotCommand(uint8_t index, uint8_t command); +void pwmWriteDshotCommand(uint8_t index, uint8_t motorCount, uint8_t command); 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); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 744c91ef23..5abefb6d16 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2339,24 +2339,18 @@ void printEscInfo(const uint8_t *escInfoBytes, uint8_t bytesRead) } } -static void writeDshotCommand(uint8_t escIndex, uint8_t command) +static void executeEscInfoCommand(uint8_t escIndex) { uint8_t escInfoBuffer[ESC_INFO_V2_EXPECTED_FRAME_SIZE]; - if (command == DSHOT_CMD_ESC_INFO) { - cliPrintLinef("Info for ESC %d:", escIndex); + cliPrintLinef("Info for ESC %d:", escIndex); - delay(10); // Wait for potential ESC telemetry transmission to finish + startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE); - startEscDataRead(escInfoBuffer, ESC_INFO_V2_EXPECTED_FRAME_SIZE); - } + pwmWriteDshotCommand(escIndex, getMotorCount(), DSHOT_CMD_ESC_INFO); - pwmWriteDshotCommand(escIndex, command); + delay(5); - if (command == DSHOT_CMD_ESC_INFO) { - delay(10); - - printEscInfo(escInfoBuffer, getNumberEscBytesRead()); - } + printEscInfo(escInfoBuffer, getNumberEscBytesRead()); } static void cliDshotProg(char *cmdline) @@ -2385,18 +2379,26 @@ static void cliDshotProg(char *cmdline) int command = atoi(pch); if (command >= 0 && command < DSHOT_MIN_THROTTLE) { - if (escIndex == ALL_MOTORS) { - for (unsigned i = 0; i < getMotorCount(); i++) { - writeDshotCommand(i, command); - } + if (command == DSHOT_CMD_ESC_INFO) { + delay(5); // Wait for potential ESC telemetry transmission to finish + } + + if (command != DSHOT_CMD_ESC_INFO) { + pwmWriteDshotCommand(escIndex, getMotorCount(), command); } else { - writeDshotCommand(escIndex, command); + if (escIndex != ALL_MOTORS) { + executeEscInfoCommand(escIndex); + } else { + for (uint8_t i = 0; i < getMotorCount(); i++) { + executeEscInfoCommand(i); + } + } } cliPrintLinef("Command %d written.", command); if (command <= 5) { - delay(10); // wait for sound output to finish + delay(20); // wait for sound output to finish } } else { cliPrintLinef("Invalid command, range 1 to %d.", DSHOT_MIN_THROTTLE - 1); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index e4eab1a982..9a473c2048 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -264,14 +264,10 @@ void tryArm(void) if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXDSHOTREVERSE)) { if (!IS_RC_MODE_ACTIVE(BOXDSHOTREVERSE)) { reverseMotors = false; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_NORMAL); - } + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL); } else { reverseMotors = true; - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_SPIN_DIRECTION_REVERSED); - } + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED); } } #endif diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 4e11c84e15..12076dd9b2 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -102,8 +102,6 @@ PG_DECLARE(motorConfig_t, motorConfig); #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF -#define ALL_MOTORS 255 - extern const mixer_t mixers[]; extern float motor[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 1cfec42e95..6a1486b975 100755 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -364,9 +364,7 @@ void beeperUpdate(timeUs_t currentTimeUs) #ifdef USE_DSHOT if (!ARMING_FLAG(ARMED) && beeperConfig()->dshotForward && currentBeeperEntry->mode == BEEPER_RX_SET) { - for (unsigned index = 0; index < getMotorCount(); index++) { - pwmWriteDshotCommand(index, DSHOT_CMD_BEEP3); - } + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_BEEP3); } #endif diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 1ac808c1e8..898af2a79a 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -285,7 +285,7 @@ void escSensorProcess(timeUs_t currentTimeUs) { const timeMs_t currentTimeMs = currentTimeUs / 1000; - if (!escSensorPort) { + if (!escSensorPort || !pwmAreMotorsEnabled()) { return; }