diff --git a/lib/EscDriver/src/DshotCommands.hpp b/lib/EscDriver/src/DshotCommands.hpp new file mode 100644 index 00000000..08e56506 --- /dev/null +++ b/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 +}; diff --git a/lib/EscDriver/src/EscDriverBase.cpp b/lib/EscDriver/src/EscDriverBase.cpp index c3cf4aea..3bf2537a 100644 --- a/lib/EscDriver/src/EscDriverBase.cpp +++ b/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; diff --git a/lib/EscDriver/src/EscDriverBase.hpp b/lib/EscDriver/src/EscDriverBase.hpp index c8c590a6..a9ec0bd8 100644 --- a/lib/EscDriver/src/EscDriverBase.hpp +++ b/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) diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 065af8ab..5745458f 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/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); diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 742f260d..1aa4cdf5 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -5,6 +5,8 @@ #include "EscDriver.h" #include +#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; diff --git a/lib/Espfc/src/Connect/Cli.cpp b/lib/Espfc/src/Connect/Cli.cpp index d7a2cd07..d4a399f4 100644 --- a/lib/Espfc/src/Connect/Cli.cpp +++ b/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]) diff --git a/lib/betaflight/src/msp/msp_protocol_v2_betaflight.h b/lib/betaflight/src/msp/msp_protocol_v2_betaflight.h index ba07b5b7..564a6859 100644 --- a/lib/betaflight/src/msp/msp_protocol_v2_betaflight.h +++ b/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 diff --git a/platformio.ini b/platformio.ini index fc1322e4..ebee03ce 100644 --- a/platformio.ini +++ b/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