From 81c226e7698f675ad92a8f208dc5911b1fd6011b Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sat, 16 Mar 2019 14:05:30 -0400 Subject: [PATCH] Periodically try to activate DSHOT telemetry if enabled but not working If DSHOT telemetry is enabled but one or more ESC's are not supplying valid telemetry packets, then send the DSHOT command to enable telemetry once a second while disarmed until all ESC's are supplying telemetry. Addresses the issue of the flight controller booting without the ESC's powered. In this case the initial command at boot to enable bidirectional telemetry will be missed by the ESC since they're not powered. If the battery is subsequently plugged in the ESC's will default to bidirectional telemetry disabled. This change will detect that ESC's are not supplying telemetry and attempt to preiodically enable them. --- src/main/drivers/pwm_output.h | 2 +- src/main/drivers/pwm_output_dshot_shared.c | 4 ++-- src/main/fc/core.c | 13 +++---------- src/main/fc/init.c | 14 +++++--------- src/main/flight/mixer.c | 12 ++++++++++++ src/main/flight/mixer.h | 2 +- 6 files changed, 24 insertions(+), 23 deletions(-) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 4cea1e15c..62bd4b487 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -254,7 +254,7 @@ bool pwmDshotCommandIsProcessing(void); uint8_t pwmGetDshotCommand(uint8_t index); bool pwmDshotCommandOutputIsEnabled(uint8_t motorCount); uint16_t getDshotTelemetry(uint8_t index); -bool isDshotTelemetryActive(uint8_t index); +bool isDshotMotorTelemetryActive(uint8_t motorIndex); #endif diff --git a/src/main/drivers/pwm_output_dshot_shared.c b/src/main/drivers/pwm_output_dshot_shared.c index 67cffeef4..c2c250554 100644 --- a/src/main/drivers/pwm_output_dshot_shared.c +++ b/src/main/drivers/pwm_output_dshot_shared.c @@ -247,9 +247,9 @@ void pwmStartDshotMotorUpdate(uint8_t motorCount) } } -bool isDshotTelemetryActive(uint8_t index) +bool isDshotMotorTelemetryActive(uint8_t motorIndex) { - return dmaMotors[index].dshotTelemetryActive; + return dmaMotors[motorIndex].dshotTelemetryActive; } #endif diff --git a/src/main/fc/core.c b/src/main/fc/core.c index d840e2167..ef46fff71 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -294,18 +294,11 @@ void updateArmingStatus(void) #endif #ifdef USE_RPM_FILTER - // USE_RPM_FILTER will only be set if USE_DSHOT and USE_DSHOT_TELEMETRY are defined - // If dshot_bidir is anabled and any motor isn't providing telemetry, then disable arming + // USE_RPM_FILTER will only be defined if USE_DSHOT and USE_DSHOT_TELEMETRY are defined + // If the RPM filter is anabled and any motor isn't providing telemetry, then disable arming if (motorConfig()->dev.useDshotTelemetry && (rpmFilterConfig()->gyro_rpm_notch_harmonics || rpmFilterConfig()->dterm_rpm_notch_harmonics)) { - bool dshotTelemetryActive = true; - for (uint8_t i = 0; i < getMotorCount(); i++) { - if (!isDshotTelemetryActive(i)) { - dshotTelemetryActive = false; - break; - } - } - if (!dshotTelemetryActive) { + if (!isDshotTelemetryActive()) { setArmingDisabled(ARMING_DISABLED_RPMFILTER); } else { unsetArmingDisabled(ARMING_DISABLED_RPMFILTER); diff --git a/src/main/fc/init.c b/src/main/fc/init.c index e5a7a51d6..156560d17 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -209,15 +209,12 @@ static IO_t busSwitchResetPin = IO_NONE; } #endif -#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) +#ifdef USE_DSHOT_TELEMETRY void activateDshotTelemetry(struct dispatchEntry_s* self) { - UNUSED(self); - if (!ARMING_FLAG(ARMED)) - { - pwmWriteDshotCommand( - 255, getMotorCount(), motorConfig()->dev.useDshotTelemetry ? - DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY : DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE, false); + if (!ARMING_FLAG(ARMED) && !isDshotTelemetryActive()) { + pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY, false); + dispatchAdd(self, 1e6); // check again in 1 second } } @@ -811,8 +808,7 @@ void init(void) setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME); -// TODO: potentially delete when feature is stable. Activation when arming is enough for flight. -#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) +#ifdef USE_DSHOT_TELEMETRY if (motorConfig()->dev.useDshotTelemetry) { dispatchEnable(); dispatchAdd(&activateDshotTelemetryEntry, 5000000); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d81c543bb..b498440a9 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -1037,3 +1037,15 @@ float mixerGetLoggingThrottle(void) { return loggingThrottle; } + +#ifdef USE_DSHOT_TELEMETRY +bool isDshotTelemetryActive(void) +{ + for (uint8_t i = 0; i < motorCount; i++) { + if (!isDshotMotorTelemetryActive(i)) { + return false; + } + } + return true; +} +#endif diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 6cbce457c..64da4d69d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -132,4 +132,4 @@ bool mixerIsTricopter(void); void mixerSetThrottleAngleCorrection(int correctionValue); float mixerGetLoggingThrottle(void); - +bool isDshotTelemetryActive(void); -- 2.11.4.GIT