src/main/drivers/dshot_command.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include "platform.h"
  25
  26 #ifdef USE_DSHOT
  27
  28 #include "common/time.h"
  29
  30 #include "drivers/io.h"
  31 #include "drivers/motor.h"
  32 #include "drivers/time.h"
  33 #include "drivers/timer.h"
  34
  35 #include "drivers/dshot.h"
  36 #include "drivers/dshot_dpwm.h"
  37 #include "drivers/pwm_output.h"
  38
  39 #include "dshot_command.h"
  40
  41 #define DSHOT_PROTOCOL_DETECTION_DELAY_MS 3000
  42 #define DSHOT_INITIAL_DELAY_US 10000
  43 #define DSHOT_COMMAND_DELAY_US 1000
  44 #define DSHOT_ESCINFO_DELAY_US 12000
  45 #define DSHOT_BEEP_DELAY_US 100000
  46 #define DSHOT_MAX_COMMANDS 3
  47
  48 typedef enum {
  49     DSHOT_COMMAND_STATE_IDLEWAIT,   // waiting for motors to go idle
  50     DSHOT_COMMAND_STATE_STARTDELAY, // initial delay period before a sequence of commands
  51     DSHOT_COMMAND_STATE_ACTIVE,     // actively sending the command (with optional repeated output)
  52     DSHOT_COMMAND_STATE_POSTDELAY   // delay period after the command has been sent
  53 } dshotCommandState_e;
  54
  55 typedef struct dshotCommandControl_s {
  56     dshotCommandState_e state;
  57     uint32_t nextCommandCycleDelay;
  58     timeUs_t delayAfterCommandUs;
  59     uint8_t repeats;
  60     uint8_t command[MAX_SUPPORTED_MOTORS];
  61 } dshotCommandControl_t;
  62
  63 static timeUs_t dshotCommandPidLoopTimeUs = 125; // default to 8KHz (125us) loop to prevent possible div/0
  64                                                  // gets set to the actual value when the PID loop is initialized
  65
  66 // XXX Optimization opportunity here.
  67 // https://github.com/betaflight/betaflight/pull/8534#pullrequestreview-258947278
  68 // @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)
  69 // Explicit element count will make code below simpler, but care has to be taken to avoid race conditions
  70
  71 static dshotCommandControl_t commandQueue[DSHOT_MAX_COMMANDS + 1];
  72 static uint8_t commandQueueHead;
  73 static uint8_t commandQueueTail;
  74
  75 void dshotSetPidLoopTime(uint32_t pidLoopTime)
  76 {
  77     dshotCommandPidLoopTimeUs = pidLoopTime;
  78 }
  79
  80 static FAST_CODE bool dshotCommandQueueFull()
  81 {
  82     return (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueTail;
  83 }
  84
  85 FAST_CODE bool dshotCommandQueueEmpty(void)
  86 {
  87     return commandQueueHead == commandQueueTail;
  88 }
  89
  90 static FAST_CODE bool isLastDshotCommand(void)
  91 {
  92     return ((commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1) == commandQueueHead);
  93 }
  94
  95 FAST_CODE bool dshotCommandIsProcessing(void)
  96 {
  97     if (dshotCommandQueueEmpty()) {
  98         return false;
  99     }
 100     dshotCommandControl_t* command = &commandQueue[commandQueueTail];
 101     const bool commandIsProcessing = command->state == DSHOT_COMMAND_STATE_STARTDELAY
 102                                      || command->state == DSHOT_COMMAND_STATE_ACTIVE
 103                                      || (command->state == DSHOT_COMMAND_STATE_POSTDELAY && !isLastDshotCommand());
 104     return commandIsProcessing;
 105 }
 106
 107 static FAST_CODE bool dshotCommandQueueUpdate(void)
 108 {
 109     if (!dshotCommandQueueEmpty()) {
 110         commandQueueTail = (commandQueueTail + 1) % (DSHOT_MAX_COMMANDS + 1);
 111         if (!dshotCommandQueueEmpty()) {
 112             // There is another command in the queue so update it so it's ready to output in
 113             // sequence. It can go directly to the DSHOT_COMMAND_STATE_ACTIVE state and bypass
 114             // the DSHOT_COMMAND_STATE_IDLEWAIT and DSHOT_COMMAND_STATE_STARTDELAY states.
 115             dshotCommandControl_t* nextCommand = &commandQueue[commandQueueTail];
 116             nextCommand->state = DSHOT_COMMAND_STATE_ACTIVE;
 117             nextCommand->nextCommandCycleDelay = 0;
 118             return true;
 119         }
 120     }
 121     return false;
 122 }
 123
 124 static FAST_CODE uint32_t dshotCommandCyclesFromTime(timeUs_t delayUs)
 125 {
 126     // Find the minimum number of motor output cycles needed to
 127     // provide at least delayUs time delay
 128
 129     return (delayUs + dshotCommandPidLoopTimeUs - 1) / dshotCommandPidLoopTimeUs;
 130 }
 131
 132 static dshotCommandControl_t* addCommand()
 133 {
 134     int newHead = (commandQueueHead + 1) % (DSHOT_MAX_COMMANDS + 1);
 135     if (newHead == commandQueueTail) {
 136         return NULL;
 137     }
 138     dshotCommandControl_t* control = &commandQueue[commandQueueHead];
 139     commandQueueHead = newHead;
 140     return control;
 141 }
 142
 143 static bool allMotorsAreIdle(void)
 144 {
 145     for (unsigned i = 0; i < motorDeviceCount(); i++) {
 146         const motorDmaOutput_t *motor = getMotorDmaOutput(i);
 147         if (motor->protocolControl.value) {
 148             return false;
 149         }
 150     }
 151
 152     return true;
 153 }
 154
 155 bool dshotStreamingCommandsAreEnabled(void)
 156 {
 157     return motorIsEnabled() && motorGetMotorEnableTimeMs() && millis() > motorGetMotorEnableTimeMs() + DSHOT_PROTOCOL_DETECTION_DELAY_MS;
 158 }
 159
 160 static bool dshotCommandsAreEnabled(dshotCommandType_e commandType)
 161 {
 162     bool ret = false;
 163
 164     switch (commandType) {
 165     case DSHOT_CMD_TYPE_BLOCKING:
 166         ret = !motorIsEnabled();
 167
 168         break;
 169     case DSHOT_CMD_TYPE_INLINE:
 170         ret = dshotStreamingCommandsAreEnabled();
 171
 172         break;
 173     default:
 174
 175         break;
 176     }
 177
 178     return ret;
 179 }
 180
 181 void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshotCommandType_e commandType)
 182 {
 183     if (!isMotorProtocolDshot() || !dshotCommandsAreEnabled(commandType) || (command > DSHOT_MAX_COMMAND) || dshotCommandQueueFull()) {
 184         return;
 185     }
 186
 187     uint8_t repeats = 1;
 188     timeUs_t delayAfterCommandUs = DSHOT_COMMAND_DELAY_US;
 189
 190     switch (command) {
 191     case DSHOT_CMD_SPIN_DIRECTION_1:
 192     case DSHOT_CMD_SPIN_DIRECTION_2:
 193     case DSHOT_CMD_3D_MODE_OFF:
 194     case DSHOT_CMD_3D_MODE_ON:
 195     case DSHOT_CMD_SAVE_SETTINGS:
 196     case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
 197     case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
 198         repeats = 10;
 199         break;
 200     case DSHOT_CMD_BEACON1:
 201     case DSHOT_CMD_BEACON2:
 202     case DSHOT_CMD_BEACON3:
 203     case DSHOT_CMD_BEACON4:
 204     case DSHOT_CMD_BEACON5:
 205         delayAfterCommandUs = DSHOT_BEEP_DELAY_US;
 206         break;
 207     default:
 208         break;
 209     }
 210
 211     if (commandType == DSHOT_CMD_TYPE_BLOCKING) {
 212         delayMicroseconds(DSHOT_INITIAL_DELAY_US - DSHOT_COMMAND_DELAY_US);
 213         for (; repeats; repeats--) {
 214             delayMicroseconds(DSHOT_COMMAND_DELAY_US);
 215
 216 #ifdef USE_DSHOT_TELEMETRY
 217             timeUs_t timeoutUs = micros() + 1000;
 218             while (!motorGetVTable().updateStart() &&
 219                    cmpTimeUs(timeoutUs, micros()) > 0);
 220 #endif
 221             for (uint8_t i = 0; i < motorDeviceCount(); i++) {
 222                 if ((i == index) || (index == ALL_MOTORS)) {
 223                     motorDmaOutput_t *const motor = getMotorDmaOutput(i);
 224                     motor->protocolControl.requestTelemetry = true;
 225                     motorGetVTable().writeInt(i, command);
 226                 }
 227             }
 228
 229             motorGetVTable().updateComplete();
 230         }
 231         delayMicroseconds(delayAfterCommandUs);
 232     } else if (commandType == DSHOT_CMD_TYPE_INLINE) {
 233         dshotCommandControl_t *commandControl = addCommand();
 234         if (commandControl) {
 235             commandControl->repeats = repeats;
 236             commandControl->delayAfterCommandUs = delayAfterCommandUs;
 237             for (unsigned i = 0; i < motorCount; i++) {
 238                 if (index == i || index == ALL_MOTORS) {
 239                     commandControl->command[i] = command;
 240                 } else {
 241                     commandControl->command[i] = DSHOT_CMD_MOTOR_STOP;
 242                 }
 243             }
 244             if (allMotorsAreIdle()) {
 245                 // we can skip the motors idle wait state
 246                 commandControl->state = DSHOT_COMMAND_STATE_STARTDELAY;
 247                 commandControl->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
 248             } else {
 249                 commandControl->state = DSHOT_COMMAND_STATE_IDLEWAIT;
 250                 commandControl->nextCommandCycleDelay = 0;  // will be set after idle wait completes
 251             }
 252         }
 253     }
 254 }
 255
 256 uint8_t dshotCommandGetCurrent(uint8_t index)
 257 {
 258     return commandQueue[commandQueueTail].command[index];
 259 }
 260
 261 // This function is used to synchronize the dshot command output timing with
 262 // the normal motor output timing tied to the PID loop frequency. A "true" result
 263 // allows the motor output to be sent, "false" means delay until next loop. So take
 264 // the example of a dshot command that needs to repeat 10 times at 1ms intervals.
 265 // If we have a 8KHz PID loop we'll end up sending the dshot command every 8th motor output.
 266 FAST_CODE_NOINLINE bool dshotCommandOutputIsEnabled(uint8_t motorCount)
 267 {
 268     UNUSED(motorCount);
 269
 270     dshotCommandControl_t* command = &commandQueue[commandQueueTail];
 271     switch (command->state) {
 272     case DSHOT_COMMAND_STATE_IDLEWAIT:
 273         if (allMotorsAreIdle()) {
 274             command->state = DSHOT_COMMAND_STATE_STARTDELAY;
 275             command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_INITIAL_DELAY_US);
 276         }
 277         break;
 278
 279     case DSHOT_COMMAND_STATE_STARTDELAY:
 280         if (command->nextCommandCycleDelay) {
 281             --command->nextCommandCycleDelay;
 282             return false;  // Delay motor output until the start of the command sequence
 283         }
 284         command->state = DSHOT_COMMAND_STATE_ACTIVE;
 285         command->nextCommandCycleDelay = 0;  // first iteration of the repeat happens now
 286         FALLTHROUGH;
 287
 288     case DSHOT_COMMAND_STATE_ACTIVE:
 289         if (command->nextCommandCycleDelay) {
 290             --command->nextCommandCycleDelay;
 291             return false;  // Delay motor output until the next command repeat
 292         }
 293
 294         command->repeats--;
 295         if (command->repeats) {
 296             command->nextCommandCycleDelay = dshotCommandCyclesFromTime(DSHOT_COMMAND_DELAY_US);
 297         } else {
 298             command->state = DSHOT_COMMAND_STATE_POSTDELAY;
 299             command->nextCommandCycleDelay = dshotCommandCyclesFromTime(command->delayAfterCommandUs);
 300             if (!isLastDshotCommand() && command->nextCommandCycleDelay > 0) {
 301                 // Account for the 1 extra motor output loop between commands.
 302                 // Otherwise the inter-command delay will be DSHOT_COMMAND_DELAY_US + 1 loop.
 303                 command->nextCommandCycleDelay--;
 304             }
 305         }
 306         break;
 307
 308     case DSHOT_COMMAND_STATE_POSTDELAY:
 309         if (command->nextCommandCycleDelay) {
 310             --command->nextCommandCycleDelay;
 311             return false;  // Delay motor output until the end of the post-command delay
 312         }
 313         if (dshotCommandQueueUpdate()) {
 314             // Will be true if the command queue is not empty and we
 315             // want to wait for the next command to start in sequence.
 316             return false;
 317         }
 318     }
 319
 320     return true;
 321 }
 322 #endif // USE_DSHOT