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

@@ -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
 

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

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);

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);

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

src/main/flight/mixer.h

@@ -132,4 +132,4 @@ bool mixerIsTricopter(void);
 
 void mixerSetThrottleAngleCorrection(int correctionValue);
 float mixerGetLoggingThrottle(void);
-
+bool isDshotTelemetryActive(void);