Motor code refactor (Phase 1)

src/main/drivers/dshot_command.c

@@ -0,0 +1,298 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_DSHOT
+
+#include "common/time.h"
+
+#include "drivers/io.h"
+#include "drivers/motor.h"
+#include "drivers/time.h"
+#include "drivers/timer.h"
+
+#include "drivers/dshot.h"
+#include "drivers/dshot_dpwm.h"
+#include "drivers/dshot_command.h"
+#include "drivers/pwm_output.h"
+
+#define DSHOT_INITIAL_DELAY_US 10000
+#define DSHOT_COMMAND_DELAY_US 1000
+#define DSHOT_ESCINFO_DELAY_US 12000
+#define DSHOT_BEEP_DELAY_US 100000
+#define DSHOT_MAX_COMMANDS 3
+
+typedef enum {
+    DSHOT_COMMAND_STATE_IDLEWAIT,   // waiting for motors to go idle
+    DSHOT_COMMAND_STATE_STARTDELAY, // initial delay period before a sequence of commands
+    DSHOT_COMMAND_STATE_ACTIVE,     // actively sending the command (with optional repeated output)
+    DSHOT_COMMAND_STATE_POSTDELAY   // delay period after the command has been sent
+} dshotCommandState_e;
+
+typedef struct dshotCommandControl_s {
+    dshotCommandState_e state;
+    uint32_t nextCommandCycleDelay;
+    timeUs_t delayAfterCommandUs;
+    uint8_t repeats;
+    uint8_t command[MAX_SUPPORTED_MOTORS];
+} dshotCommandControl_t;
+
+static timeUs_t dshotCommandPidLoopTimeUs = 125; // default to 8KHz (125us) loop to prevent possible div/0
+                                                 // gets set to the actual value when the PID loop is initialized
+
+// XXX Optimization opportunity here.
+// https://github.com/betaflight/betaflight/pull/8534#pullrequestreview-258947278
+// @ledvinap: queue entry is quite large - it may be better to handle empty/full queue using different mechanism (magic value for Head or Tail / explicit element count)
+// Explicit element count will make code below simpler, but care has to be taken to avoid race conditions
+
+static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1];
+static uint8_t commandQueueHead;
+static uint8_t commandQueueTail;
+
+void dshotSetPidLoopTime(uint32_t pidLoopTime)
+{
+    dshotCommandPidLoopTimeUs = pidLoopTime;
+}
+
+static FAST_CODE bool dshotCommandQueueFull()
+{   
+    return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail;
+}   
+    
+FAST_CODE bool dshotCommandQueueEmpty(void)
+{   
+    return commandQueueHead == commandQueueTail;
+}       
+
+static FAST_CODE bool isLastDshotCommand(void)
+{   
+    return ((commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueHead);
+}   
+    
+FAST_CODE bool dshotCommandIsProcessing(void)
+{       
+    if (dshotCommandQueueEmpty()) {
+        return false;
+    }
+    dshotCommandControl_t* command = &commandQueue[commandQueueTail];
+    const bool commandIsProcessing = command->state == DSHOT_COMMAND_STATE_STARTDELAY
+                                     || command->state == DSHOT_COMMAND_STATE_ACTIVE   
+                                     || (command->state == DSHOT_COMMAND_STATE_POSTDELAY && !isLastDshotCommand());
+    return commandIsProcessing;
+}
+
+static FAST_CODE bool dshotCommandQueueUpdate(void)
+{   
+    if (!dshotCommandQueueEmpty()) {
+        commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1);
+        if (!dshotCommandQueueEmpty()) {
+            // There is another command in the queue so update it so it's ready to output in   
+            // sequence. It can go directly to the DSHOT_COMMAND_STATE_ACTIVE state and bypass 
+            // the DSHOT_COMMAND_STATE_IDLEWAIT and DSHOT_COMMAND_STATE_STARTDELAY states.     
+            dshotCommandControl_t* nextCommand = &commandQueue[commandQueueTail];
+            nextCommand->state = DSHOT_COMMAND_STATE_ACTIVE;
+            nextCommand->nextCommandCycleDelay = 0;
+            return true;
+        }
+    }
+    return false;
+}
+
+static FAST_CODE uint32_t dshotCommandCyclesFromTime(timeUs_t delayUs)
+{
+    // Find the minimum number of motor output cycles needed to
+    // provide at least delayUs time delay
+
+    return (delayUs + dshotCommandPidLoopTimeUs - 1) / dshotCommandPidLoopTimeUs;
+}
+
+static dshotCommandControl_t* addCommand()
+{
+    int newHead = (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1);
+    if (newHead == commandQueueTail) {
+        return NULL;
+    }
+    dshotCommandControl_t* control = &commandQueue[commandQueueHead];
+    commandQueueHead = newHead;
+    return control;
+}
+
+static bool allMotorsAreIdle(void)
+{
+    for (unsigned i = 0; i < dshotPwmDevice.count; i++) {
+        const motorDmaOutput_t *motor = getMotorDmaOutput(i);
+        if (motor->protocolControl.value) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, bool blocking)
+{
+    UNUSED(motorCount);
+
+    if (!isMotorProtocolDshot() || (command > DSHOT_MAX_COMMAND) || dshotCommandQueueFull()) {
+        return;
+    }
+
+    uint8_t repeats = 1;
+    timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
+
+    switch (command) {
+    case DSHOT_CMD_SPIN_DIRECTION_1:
+    case DSHOT_CMD_SPIN_DIRECTION_2:
+    case DSHOT_CMD_3D_MODE_OFF:
+    case DSHOT_CMD_3D_MODE_ON:
+    case DSHOT_CMD_SAVE_SETTINGS:
+    case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
+    case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
+    case DSHOT_CMD_SIGNAL_LINE_TELEMETRY_DISABLE:
+    case DSHOT_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY:
+        repeats = 10;
+        break;
+    case DSHOT_CMD_BEACON1:
+    case DSHOT_CMD_BEACON2:
+    case DSHOT_CMD_BEACON3:
+    case DSHOT_CMD_BEACON4:
+    case DSHOT_CMD_BEACON5:
+        delayAfterCommandUs = DSHOT_BEEP_DELAY_US;
+        break;
+    default:
+        break;
+    }
+
+    if (blocking) {
+        delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US);
+        for (; repeats; repeats--) {
+            delayMicroseconds(DSHOT_COMMAND_DELAY_US);
+
+#ifdef USE_DSHOT_TELEMETRY
+            timeUs_t timeoutUs = micros() + 1000;
+            while (!pwmStartDshotMotorUpdate() &&
+                   cmpTimeUs(timeoutUs, micros()) > 0);
+#endif
+            for (uint8_t i = 0; i < dshotPwmDevice.count; i++) {
+                if ((i == index) || (index == ALL_MOTORS)) {
+                    motorDmaOutput_t *const motor = getMotorDmaOutput(i);
+                    motor->protocolControl.requestTelemetry = true;
+                    dshotPwmDevice.vTable.writeInt(i, command);
+                }
+            }
+
+            dshotPwmDevice.vTable.updateComplete();
+        }
+        delayMicroseconds(delayAfterCommandUs);
+    } else {
+        dshotCommandControl_t *commandControl = addCommand();
+        if (commandControl) {
+            commandControl->repeats = repeats;
+            commandControl->delayAfterCommandUs = delayAfterCommandUs;
+            for (unsigned i = 0; i < dshotPwmDevice.count; i++) {
+                if (index == i || index == ALL_MOTORS) {
+                    commandControl->command[i] = command;
+                } else {
+                    commandControl->command[i] = DSHOT_CMD_MOTOR_STOP;
+                }
+            }
+            if (allMotorsAreIdle()) {
+                // we can skip the motors idle wait state
+                commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY;
+                commandControl->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
+            } else {
+                commandControl->state = DSHOT_COMMAND_STATE_IDLEWAIT;
+                commandControl->nextCommandCycleDelay = 0;  // will be set after idle wait completes
+            }
+        }
+    }
+}
+
+uint8_t pwmGetDshotCommand(uint8_t index)
+{
+    return commandQueue[commandQueueTail].command[index];
+}
+
+// This function is used to synchronize the dshot command output timing with
+// the normal motor output timing tied to the PID loop frequency. A "true" result
+// allows the motor output to be sent, "false" means delay until next loop. So take
+// the example of a dshot command that needs to repeat 10 times at 1ms intervals.
+// If we have a 8KHz PID loop we'll end up sending the dshot command every 8th motor output.
+FAST_CODE_NOINLINE bool dshotCommandOutputIsEnabled(uint8_t motorCount)
+{
+    UNUSED(motorCount);
+
+    dshotCommandControl_t* command = &commandQueue[commandQueueTail];
+    switch (command->state) {
+    case DSHOT_COMMAND_STATE_IDLEWAIT:
+        if (allMotorsAreIdle()) {
+            command->state = DSHOT_COMMAND_STATE_STARTDELAY;
+            command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
+        }
+        break;
+
+    case DSHOT_COMMAND_STATE_STARTDELAY:
+        if (command->nextCommandCycleDelay) {
+            --command->nextCommandCycleDelay;
+            return false;  // Delay motor output until the start of the command seequence
+        }
+        command->state = DSHOT_COMMAND_STATE_ACTIVE;
+        command->nextCommandCycleDelay = 0;  // first iteration of the repeat happens now
+        FALLTHROUGH;
+
+    case DSHOT_COMMAND_STATE_ACTIVE:
+        if (command->nextCommandCycleDelay) {
+            --command->nextCommandCycleDelay;
+            return false;  // Delay motor output until the next command repeat
+        }
+
+        command->repeats--;
+        if (command->repeats) {
+            command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_COMMAND_DELAY_US);
+        } else {
+            command->state = DSHOT_COMMAND_STATE_POSTDELAY;
+            command->nextCommandCycleDelay = dshotCommandCyclesFromTime(command->delayAfterCommandUs);
+            if (!isLastDshotCommand() && command->nextCommandCycleDelay > 0) {
+                // Account for the 1 extra motor output loop between commands.
+                // Otherwise the inter-command delay will be DSHOT_COMMAND_DELAY_US + 1 loop.
+                command->nextCommandCycleDelay--;
+            }
+        }
+        break;
+
+    case DSHOT_COMMAND_STATE_POSTDELAY:
+        if (command->nextCommandCycleDelay) {
+            --command->nextCommandCycleDelay;
+            return false;  // Delay motor output until the end of the post-command delay
+        }
+        if (dshotCommandQueueUpdate()) {
+            // Will be true if the command queue is not empty and we
+            // want to wait for the next command to start in sequence.
+            return false;
+        }
+    }
+
+    return true;
+}
+#endif // USE_DSHOT