From 094ebad282cf68ce5f96683c74dc369c8a7d0b6c Mon Sep 17 00:00:00 2001
From: Steve Evans <Steve@SCEvans.com>
Date: Sat, 3 Jun 2023 15:20:13 +0100
Subject: [PATCH] Call updateInit() before writing motor command data

---
 src/main/drivers/dshot_command.c | 26 +++++++++++++++++++-------
 src/main/drivers/motor.c         |  4 ++--
 src/main/drivers/motor.h         |  2 +-
 3 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c
index cd04ccc65fc..3421ce019a6 100644
--- a/src/main/drivers/dshot_command.c
+++ b/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);
 
diff --git a/src/main/drivers/motor.c b/src/main/drivers/motor.c
index e604303f0d9..fc2f11731b3 100644
--- a/src/main/drivers/motor.c
+++ b/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
diff --git a/src/main/drivers/motor.h b/src/main/drivers/motor.h
index fb9846aa6fd..9ff075fe750 100644
--- a/src/main/drivers/motor.h
+++ b/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);