Adds dshot reverse commands support

lib/EscDriver/src/DshotCommands.hpp

@@ -0,0 +1,45 @@
+#pragma once
+
+#include "Arduino.h"
+#include "EscDriver.h"
+
+// The official DShot Commands
+enum DshotCommand
+{
+    DSHOT_CMD_MOTOR_STOP = 0,                    // Currently not implemented - STOP Motors
+    DSHOT_CMD_BEEP1,                             // Wait at least length of beep (380ms) before next command
+    DSHOT_CMD_BEEP2,                             // Wait at least length of beep (380ms) before next command
+    DSHOT_CMD_BEEP3,                             // Wait at least length of beep (400ms) before next command
+    DSHOT_CMD_BEEP4,                             // Wait at least length of beep (400ms) before next command
+    DSHOT_CMD_BEEP5,                             // Wait at least length of beep (400ms) before next command
+    DSHOT_CMD_ESC_INFO,                          // Currently not implemented
+    DSHOT_CMD_SPIN_DIRECTION_1,                  // Need 6x, no wait required
+    DSHOT_CMD_SPIN_DIRECTION_2,                  // Need 6x, no wait required
+    DSHOT_CMD_3D_MODE_OFF,                       // Need 6x, no wait required
+    DSHOT_CMD_3D_MODE_ON,                        // Need 6x, no wait required
+    DSHOT_CMD_SETTINGS_REQUEST,                  // Currently not implemented
+    DSHOT_CMD_SAVE_SETTINGS,                     // Need 6x, wait at least 12ms before next command
+    DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,        // Need 6x, no wait required 
+    DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,      // Need 6x, no wait required
+    DSHOT_CMD_LED0_ON,                           // Currently not implemented
+    DSHOT_CMD_LED1_ON,                           // Currently not implemented
+    DSHOT_CMD_LED2_ON,                           // Currently not implemented
+    DSHOT_CMD_LED3_ON,                           // Currently not implemented
+    DSHOT_CMD_LED0_OFF,                          // Currently not implemented
+    DSHOT_CMD_LED1_OFF,                          // Currently not implemented
+    DSHOT_CMD_LED2_OFF,                          // Currently not implemented
+    DSHOT_CMD_LED3_OFF,                          // Currently not implemented
+    DSHOT_CMD_36,                                // Not yet assigned
+    DSHOT_CMD_37,                                // Not yet assigned
+    DSHOT_CMD_38,                                // Not yet assigned
+    DSHOT_CMD_39,                                // Not yet assigned
+    DSHOT_CMD_40,                                // Not yet assigned
+    DSHOT_CMD_41,                                // Not yet assigned
+    DSHOT_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY, // No wait required
+    DSHOT_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY,     // No wait required
+    DSHOT_CMD_SIGNAL_LINE_CURRENT_TELEMETRY,     // No wait required
+    DSHOT_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY, // No wait required
+    DSHOT_CMD_SIGNAL_LINE_ERPM_TELEMETRY,        // No wait required
+    DSHOT_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY, // No wait required (also command 47)
+    DSHOT_CMD_MAX = 47
+};

lib/EscDriver/src/EscDriverBase.cpp

@@ -27,6 +27,14 @@ uint16_t IRAM_ATTR EscDriverBase::dshotEncode(uint16_t value, bool inverted)
   return (value << 4) | csum;
 }
 
+uint16_t IRAM_ATTR EscDriverBase::buildDshotFrame(uint16_t command, bool telemetry) 
+{
+  command &= 0x7FF; // 11-bit command
+  uint16_t frame = (command << 5) | (telemetry ? 0x10 : 0);
+  uint8_t crc = ((frame >> 12) ^ (frame >> 8) ^ (frame >> 4)) & 0x0F;
+  return (frame & 0xFFF0) | crc;
+}
+
 uint32_t IRAM_ATTR EscDriverBase::durationToBitLen(uint32_t duration, uint32_t len)
 {
   return (duration + (len >> 1)) / len;

lib/EscDriver/src/EscDriverBase.hpp

@@ -39,6 +39,7 @@ class EscDriverBase
 
     static uint16_t dshotConvert(uint16_t pulse);
     static uint16_t dshotEncode(uint16_t value, bool inverted = false);
+    static uint16_t buildDshotFrame(uint16_t command, bool telemetry);
 
     /**
      * @param data expected data layout (bits): duration0(15), level0(1), duration(15), level1(1)

lib/EscDriver/src/EscDriverEsp32.cpp

@@ -109,11 +109,50 @@ int EscDriverEsp32::attach(size_t channel, int pin, int pulse)
 
 int IRAM_ATTR EscDriverEsp32::write(size_t channel, int pulse)
 {
-  if (channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0;
+  if (_holdThrottle) Serial.println("hold throttle");
+  if (channel < 0 || channel >= ESC_CHANNEL_COUNT || _holdThrottle) return 0;
   _channel[channel].pulse = pulse;
   return 1;
 }
 
+int EscDriverEsp32::reverseMotor(size_t channel, bool reverse)
+{
+  for (int i = 0; i < 30; i++)
+  {
+    uint16_t frame = buildDshotFrame(0, true);
+    Slot& slot = _channel[channel];
+    for (size_t i = 0; i < DSHOT_BIT_COUNT; i++)
+    {
+      int val = (frame >> (DSHOT_BIT_COUNT - 1 - i)) & 0x01;
+      slot.setDshotBit(i, val, _dshot_tlm);
+      //Serial.print(val);
+    }
+    //Serial.println();
+    slot.setTerminate(DSHOT_BIT_COUNT, _dshot_tlm);
+    _rmt_fill_tx_items((rmt_channel_t)channel, slot.items, Slot::ITEM_COUNT, 0);
+
+    transmitCommand(channel);
+    delayMicroseconds(100);
+
+  }
+  delayMicroseconds(1000);
+
+  _holdThrottle = true;
+  if (channel < 0 || channel >= ESC_CHANNEL_COUNT || !isDigital(_protocol)) return 0;
+  if (reverse) {
+    Serial.println("reverse");
+    writeDshotCustomCommand(channel, DSHOT_CMD_SPIN_DIRECTION_2);
+  } else {
+    Serial.println("forward");
+    writeDshotCustomCommand(channel, DSHOT_CMD_SPIN_DIRECTION_1);
+  }
+
+
+  writeDshotCustomCommand(channel, DSHOT_CMD_SAVE_SETTINGS);
+
+  return 1;
+}
+
 void IRAM_ATTR EscDriverEsp32::apply()
 {
   if (_protocol == ESC_PROTOCOL_DISABLED) return;
@@ -383,6 +422,7 @@ void IRAM_ATTR EscDriverEsp32::writeAnalogCommand(uint32_t channel, int32_t puls
 
 void IRAM_ATTR EscDriverEsp32::writeDshotCommand(uint32_t channel, int32_t pulse)
 {
+  if (_holdThrottle) Serial.println("Holding throttle");
   if(_digital && _dshot_tlm)
   {
     modeTx((rmt_channel_t)channel);
@@ -404,6 +444,56 @@ void IRAM_ATTR EscDriverEsp32::writeDshotCommand(uint32_t channel, int32_t pulse
   _rmt_fill_tx_items((rmt_channel_t)channel, slot.items, Slot::ITEM_COUNT, 0);
 }
 
+void IRAM_ATTR EscDriverEsp32::writeDshotCustomCommand(uint32_t channel, DshotCommand cmd)
+{
+  if (!_digital || channel >= ESC_CHANNEL_COUNT) return;
+
+  uint16_t frame;
+  switch (cmd) {
+      case DSHOT_CMD_SPIN_DIRECTION_1:
+          frame = buildDshotFrame(DSHOT_CMD_SPIN_DIRECTION_1, _dshot_tlm); // Normal direction
+          Serial.printf("Channel %d: SPIN_DIRECTION_1, frame: 0x%04x\n", channel, frame);
+          break;
+      case DSHOT_CMD_SPIN_DIRECTION_2:
+          frame = buildDshotFrame(DSHOT_CMD_SPIN_DIRECTION_2, _dshot_tlm); // Reverse direction
+          Serial.printf("Channel %d: SPIN_DIRECTION_2, frame: 0x%04x\n", channel, frame);
+          break;
+      case DSHOT_CMD_SAVE_SETTINGS:
+          frame = buildDshotFrame(DSHOT_CMD_SAVE_SETTINGS, _dshot_tlm); // Save settings
+          Serial.printf("Channel %d: SAVE_SETTINGS, frame: 0x%04x\n", channel, frame);
+          break;
+      default:
+          return;
+  }
+
+  // Stop throttle
+  _holdThrottle = true;
+  int savedPulse = _channel[channel].pulse;
+  _channel[channel].pulse = 0; // Ensure throttle is 0
+
+  // Send command 50 times (100ms total at 2ms intervals)
+  for (int i = 0; i < 50; i++) {
+      Slot& slot = _channel[channel];
+      for (size_t j = 0; j < DSHOT_BIT_COUNT; j++) {
+          int val = (frame >> (DSHOT_BIT_COUNT - 1 - j)) & 0x01;
+          slot.setDshotBit(j, val, _dshot_tlm);
+      }
+      slot.setTerminate(DSHOT_BIT_COUNT, _dshot_tlm);
+      _rmt_fill_tx_items((rmt_channel_t)channel, slot.items, Slot::ITEM_COUNT, 0);
+      transmitCommand(channel);
+      delayMicroseconds(2000); // 500 Hz rate
+  }
+
+  // Special handling for SAVE_SETTINGS
+  if (cmd == DSHOT_CMD_SAVE_SETTINGS) {
+      delayMicroseconds(100000); // 100ms delay for EEPROM write
+  }
+
+  // Restore throttle
+  _channel[channel].pulse = savedPulse;
+  _holdThrottle = false;
+}
+
 void IRAM_ATTR EscDriverEsp32::transmitCommand(uint32_t channel)
 {
   _rmt_tx_start((rmt_channel_t)channel, true);

lib/EscDriver/src/EscDriverEsp32.h

@@ -5,6 +5,8 @@
 
 #include "EscDriver.h"
 #include <driver/rmt.h>
+#include "Utils/Timer.h"
+#include "DshotCommands.hpp"
 
 class EscDriverEsp32: public EscDriverBase
 {
@@ -82,6 +84,7 @@ class EscDriverEsp32: public EscDriverBase
     void end();
     int attach(size_t channel, int pin, int pulse);
     int write(size_t channel, int pulse);
+    int reverseMotor(size_t channel, bool reverse);
     void apply();
     int pin(size_t channel) const;
     uint32_t telemetry(size_t channel) const;
@@ -100,6 +103,7 @@ class EscDriverEsp32: public EscDriverBase
     void readTelemetry();
     void writeAnalogCommand(uint32_t channel, int32_t pulse);
     void writeDshotCommand(uint32_t channel, int32_t pulse);
+    void writeDshotCustomCommand(uint32_t channel, DshotCommand cmd);
     void transmitCommand(uint32_t channel);
     uint32_t getClockDivider() const;
     uint32_t getPulseMin() const;
@@ -115,6 +119,7 @@ class EscDriverEsp32: public EscDriverBase
     int32_t _interval;
     bool _digital;
     bool _dshot_tlm;
+    bool _holdThrottle;
     uint32_t _channel_mask;
 
     static bool _tx_end_installed;

lib/Espfc/src/Connect/Cli.cpp

@@ -940,6 +940,54 @@ void Cli::execute(CliCmd& cmd, Stream& s)
     }
     s.println();
   }
+  else if(strcmp_P(cmd.args[0], PSTR("motor")) ==0)
+  {
+    if(!cmd.args[1])
+    {
+      s.println(F("command required, available commands are"));
+      s.println(F("  reverse, normalize"));
+    }
+    else if(strcmp_P(cmd.args[1], PSTR("reverse")) == 0)
+    {
+      if(!cmd.args[2])
+      {
+        s.println(F("motor index required"));
+      }
+      else
+      {
+        int index = atoi(cmd.args[2]);
+        if(index >= 0 && index < MAX_SUPPORTED_MOTORS)
+        {
+          if(_model.state.mixer.escMotor) _model.state.mixer.escMotor->reverseMotor(index, true);
+          s.print(F("motor ")); s.print(index); s.println(F(" reversed"));
+        }
+        else
+        {
+          s.print(F("motor index out of range: ")); s.println(index);
+        }
+      }
+    }
+    else if(strcmp_P(cmd.args[1], PSTR("normalize")) == 0)
+    {
+      if(!cmd.args[2])
+      {
+        s.println(F("motor index required"));
+      }
+      else
+      {
+        int index = atoi(cmd.args[2]);
+        if(index >= 0 && index < MAX_SUPPORTED_MOTORS)
+        {
+          if(_model.state.mixer.escMotor) _model.state.mixer.escMotor->reverseMotor(index, false);
+          s.print(F("motor ")); s.print(index); s.println(F(" normalized: "));
+        }
+        else
+        {
+          s.print(F("motor index out of range: ")); s.println(index);
+        }
+      }
+    }
+  }
   else if(strcmp_P(cmd.args[0], PSTR("set")) == 0)
   {
     if(!cmd.args[1])

lib/betaflight/src/msp/msp_protocol_v2_betaflight.h

@@ -21,3 +21,4 @@
 #define MSP2_BETAFLIGHT_BIND                0x3000
 #define MSP2_MOTOR_OUTPUT_REORDERING        0x3001
 #define MSP2_SET_MOTOR_OUTPUT_REORDERING    0x3002
+#define MSP2_SEND_DSHOT_COMMAND             0x3003

platformio.ini

@@ -32,7 +32,7 @@ build_unflags =
   -ggdb
 
 monitor_speed = 115200
-upload_speed = 921600
+upload_speed = 460800
 ; monitor_filters = esp8266_exception_decoder
 ; monitor_filters = esp32_exception_decoder