diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c index 75c8cec1b7..6d83ea88d0 100644 --- a/src/main/drivers/dshot.c +++ b/src/main/drivers/dshot.c @@ -23,6 +23,7 @@ #include #include #include +#include #include "platform.h" @@ -178,6 +179,11 @@ dshotTelemetryType_t dshot_get_telemetry_type_to_decode(uint8_t motorIndex) return type; } +void dshotCleanTelemetryData(void) +{ + memset(dshotTelemetryState.motorState, 0, MAX_SUPPORTED_MOTORS * sizeof(dshotTelemetryMotorState_t)); +} + FAST_CODE void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t type, uint16_t value) { // Update telemetry data diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h index 3ba7b1e2a3..fb41b87de0 100644 --- a/src/main/drivers/dshot.h +++ b/src/main/drivers/dshot.h @@ -117,5 +117,6 @@ void validateAndfixMotorOutputReordering(uint8_t *array, const unsigned size); dshotTelemetryType_t dshot_get_telemetry_type_to_decode(uint8_t motorIndex); uint32_t dshot_decode_telemetry_value(uint32_t value, dshotTelemetryType_t *type); +void dshotCleanTelemetryData(void); void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t type, uint16_t value); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 004f0cdf91..e57f3e8e4e 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -509,9 +509,11 @@ void tryArm(void) return; } + #if defined(USE_ESC_SENSOR) && defined(USE_DSHOT_TELEMETRY) // Try to activate extended DSHOT telemetry only if no esc sensor exists and dshot telemetry is active if (isMotorProtocolDshot() && !featureIsEnabled(FEATURE_ESC_SENSOR) && motorConfig()->dev.useDshotTelemetry) { + dshotCleanTelemetryData(); dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE, DSHOT_CMD_TYPE_INLINE); } #endif