Call updateInit() before writing motor command data

src/main/drivers/dshot_command.c

@@ -182,6 +182,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
 
     uint8_t repeats = 1;
     timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
+    motorVTable_t *vTable = motorGetVTable();
 
     switch (command) {
     case DSHOT_CMD_SPIN_DIRECTION_1:
@@ -216,18 +217,29 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot
         for (; repeats; repeats--) {
             delayMicroseconds(DSHOT_COMMAND_DELAY_US);
 
-#ifdef USE_DSHOT_TELEMETRY
-            timeUs_t timeoutUs = micros() + 1000;
-            while (!motorGetVTable().decodeTelemetry() &&
-                   cmpTimeUs(timeoutUs, micros()) > 0);
-#endif
+            // Initialise the output buffers
+            if (vTable->updateInit) {
+                vTable->updateInit();
+            }
+
             for (uint8_t i = 0; i < motorDeviceCount(); i++) {
                 motorDmaOutput_t *const motor = getMotorDmaOutput(i);
                 motor->protocolControl.requestTelemetry = true;
-                motorGetVTable().writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP);
+                vTable->writeInt(i, (i == index || index == ALL_MOTORS) ? command : DSHOT_CMD_MOTOR_STOP);
             }
 
-            motorGetVTable().updateComplete();
+            // Don't attempt to write commands to the motors if telemetry is still being received
+            if (vTable->telemetryWait) {
+                (void)vTable->telemetryWait();
+            }
+
+            vTable->updateComplete();
+
+            // Perform the decode of the last data received
+            // New data will be received once the send of motor data, triggered above, completes
+#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
+            vTable->decodeTelemetry();
+#endif
         }
         delayMicroseconds(delayAfterCommandUs);
 

src/main/drivers/motor.c

@@ -114,9 +114,9 @@ unsigned motorDeviceCount(void)
     return motorDevice->count;
 }
 
-motorVTable_t motorGetVTable(void)
+motorVTable_t *motorGetVTable(void)
 {
-    return motorDevice->vTable;
+    return &motorDevice->vTable;
 }
 
 // This is not motor generic anymore; should be moved to analog pwm module

src/main/drivers/motor.h

@@ -86,7 +86,7 @@ uint16_t motorConvertToExternal(float motorValue);
 struct motorDevConfig_s; // XXX Shouldn't be needed once pwm_output* is really cleaned up.
 void motorDevInit(const struct motorDevConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount);
 unsigned motorDeviceCount(void);
-motorVTable_t motorGetVTable(void);
+motorVTable_t *motorGetVTable(void);
 bool checkMotorProtocolEnabled(const motorDevConfig_t *motorConfig, bool *protocolIsDshot);
 bool isMotorProtocolDshot(void);
 bool isMotorProtocolEnabled(void);