From 9007538a3b5e7b6ba905e162d68e5c19a9ef029a Mon Sep 17 00:00:00 2001 From: Spir0u <28966599+Spir0u@users.noreply.github.com> Date: Thu, 25 Nov 2021 12:17:18 +0100 Subject: [PATCH 1/6] Add DShot command file --- src/Dshot_Digital_Cmd_Spec.txt | 78 ++++++++++++++++++++++++++++++++++ 1 file changed, 78 insertions(+) create mode 100644 src/Dshot_Digital_Cmd_Spec.txt diff --git a/src/Dshot_Digital_Cmd_Spec.txt b/src/Dshot_Digital_Cmd_Spec.txt new file mode 100644 index 0000000..6221a38 --- /dev/null +++ b/src/Dshot_Digital_Cmd_Spec.txt @@ -0,0 +1,78 @@ +/************************************************************************/ +/* DShot Commands */ +/* */ +/* Source: https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ */ +/* */ +/************************************************************************/ + +0 DIGITAL_CMD_MOTOR_STOP // Currently not implemented +1 DIGITAL_CMD_BEEP1 // Wait at least length of beep (260ms) before next command +2 DIGITAL_CMD_BEEP2 // Wait at least length of beep (260ms) before next command +3 DIGITAL_CMD_BEEP3 // Wait at least length of beep (280ms) before next command +4 DIGITAL_CMD_BEEP4 // Wait at least length of beep (280ms) before next command +5 DIGITAL_CMD_BEEP5 // Wait at least length of beep (1020ms) before next command +6 DIGITAL_CMD_ESC_INFO // Wait at least 12ms before next command +7 DIGITAL_CMD_SPIN_DIRECTION_1 // Need 6x, no wait required +8 DIGITAL_CMD_SPIN_DIRECTION_2 // Need 6x, no wait required +9 DIGITAL_CMD_3D_MODE_OFF // Need 6x, no wait required +10 DIGITAL_CMD_3D_MODE_ON // Need 6x, no wait required +11 DIGITAL_CMD_SETTINGS_REQUEST // Currently not implemented +12 DIGITAL_CMD_SAVE_SETTINGS // Need 6x, wait at least 35ms before next command +20 DIGITAL_CMD_SPIN_DIRECTION_NORMAL // Need 6x, no wait required +21 DIGITAL_CMD_SPIN_DIRECTION_REVERSED // Need 6x, no wait required +22 DIGITAL_CMD_LED0_ON // No wait required +23 DIGITAL_CMD_LED1_ON // No wait required +24 DIGITAL_CMD_LED2_ON // No wait required +25 DIGITAL_CMD_LED3_ON // No wait required +26 DIGITAL_CMD_LED0_OFF // No wait required +27 DIGITAL_CMD_LED1_OFF // No wait required +28 DIGITAL_CMD_LED2_OFF // No wait required +29 DIGITAL_CMD_LED3_OFF // No wait required +30 Audio_Stream mode on/Off // Currently not implemented +31 Silent Mode on/Off // Currently not implemented +32 DIGITAL_CMD_SIGNAL_LINE_TELEMETRY_DISABLE // Need 6x, no wait required. Disables commands 42 to 47 +33 DIGITAL_CMD_SIGNAL_LINE_TELEMETRY_ENABLE // Need 6x, no wait required. Enables commands 42 to 47 +34 DIGITAL_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY // Need 6x, no wait required. Enables commands 42 to 47, and sends erpm if normal Dshot frame +35 DIGITAL_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_PERIOD_TELEMETRY // Need 6x, no wait required. Enables commands 42 to 47, and sends erpm period if normal Dshot frame +Commands above are only executed when motors are stopped +36 // Not yet assigned +37 // Not yet assigned +38 // Not yet assigned +39 // Not yet assigned +40 // Not yet assigned +41 // Not yet assigned +Commands below are executed at any time +42 DIGITAL_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY // No wait required +43 DIGITAL_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY // No wait required +44 DIGITAL_CMD_SIGNAL_LINE_CURRENT_TELEMETRY // No wait required +45 DIGITAL_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY // No wait required +46 DIGITAL_CMD_SIGNAL_LINE_ERPM_TELEMETRY // No wait required +47 DIGITAL_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY // No wait required + +The above commands are valid for Dshot and Proshot input signals + +DIGITAL_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY: 1°C per LSB +DIGITAL_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY: 10mV per LSB, 40.95V max +DIGITAL_CMD_SIGNAL_LINE_CURRENT_TELEMETRY: 100mA per LSB, 409.5A max +DIGITAL_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY: 10mAh per LSB, 40.95Ah max +DIGITAL_CMD_SIGNAL_LINE_ERPM_TELEMETRY: 100erpm per LSB, 409500erpm max +DIGITAL_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY: 16us per LSB, 65520us max TBD + + +ESC_INFO layout for BLHeli_32: +1-12: ESC SN +13: Indicates which response version is used. 254 is for BLHeli_32 version. +14: FW revision (32 = 32) +15: FW sub revision (10 = xx.1, 11 = xx.11) +16: Unused +17: Rotation direction reversed by dshot command or not (1:0) +18: 3D mode active or not (1:0) +19: Low voltage protection limit [0.1V] (255 = not capable, 0 = disabled) +20: Current protection limit [A] (255 = not capable, 0 = disabled) +21: LED0 on or not (1:0, 255 = LED0 not present) +22: LED1 on or not (1:0, 255 = LED1 not present) +23: LED2 on or not (1:0, 255 = LED2 not present) +24: LED3 on or not (1:0, 255 = LED3 not present) +25-31: Unused +32-63: ESC signature +64: CRC (same CRC as is used for telemetry) From f5da26eed5b0aff8d49886449d77fd3e454cbfc4 Mon Sep 17 00:00:00 2001 From: Spir0u <28966599+Spir0u@users.noreply.github.com> Date: Thu, 25 Nov 2021 13:36:29 +0100 Subject: [PATCH 2/6] Modified for four ESCs --- README.md | 2 + .../DShotLibraryTest_4ESCs.ino | 62 ++++ src/DShot4.cpp | 296 ++++++++++++++++++ src/DShot4.h | 39 +++ 4 files changed, 399 insertions(+) create mode 100644 examples/DShotLibraryTest/DShotLibraryTest_4ESCs.ino create mode 100644 src/DShot4.cpp create mode 100644 src/DShot4.h diff --git a/README.md b/README.md index fe3f84d..1f74248 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,4 @@ # DShot-Arduino DShot implementation for Arduino using bit-banging method + +Test with four ESCs \ No newline at end of file diff --git a/examples/DShotLibraryTest/DShotLibraryTest_4ESCs.ino b/examples/DShotLibraryTest/DShotLibraryTest_4ESCs.ino new file mode 100644 index 0000000..688028a --- /dev/null +++ b/examples/DShotLibraryTest/DShotLibraryTest_4ESCs.ino @@ -0,0 +1,62 @@ +#include + +/* + +redefine DSHOT_PORT if you want to change the default PORT + +Defaults +UNO: PORTD, available pins 0-7 (D0-D7) +Leonardo: PORTB, available pins 4-7 (D8-D11) + +e.g. +#define DSHOT_PORT PORTD +*/ + +#define M1 8 +#define M2 9 +#define M3 10 +#define M4 11 + +DShot esc(DShot::Mode::DSHOT300); + +uint16_t throttle = 0; +uint16_t target = 0; + +void setup() { + Serial.begin(115200); + + // Notice, all pins must be connected to same PORT + esc.attach(M1); + esc.setThrottle(M1, throttle); + esc.attach(M2); + esc.setThrottle(M2, throttle); + esc.attach(M3); + esc.setThrottle(M3, throttle); + esc.attach(M4); + esc.setThrottle(M4, throttle); +} + +void loop() { + if (Serial.available()>0){ + target = Serial.parseInt(); + if (target>2047) // safety measure, disarm when wrong input + target = 0; + Serial.print(target, HEX); + Serial.print("\t"); + } + if (throttle<48){ // special commands disabled + throttle = 48; + } + if (target<=48){ + esc.setThrottle(M1, target); + }else{ + if (target>throttle){ + throttle ++; + esc.setThrottle(M1, throttle); + }else if (target 0) + throttle |= 1; + uint16_t csum_data = throttle; + for (byte i=0; i<3; i++){ + csum ^= csum_data; + csum_data >>= 4; + } + csum &= 0xf; + return (throttle<<4)|csum; +} + +/****************** end of static functions *******************/ + +DShot::DShot(const enum Mode mode){ + dShotMode = mode; +} + +void DShot::attach(uint8_t pin){ + this->_packet = 0; + this->_pinMask = digitalPinToBitMask(pin); + pinMode(pin, OUTPUT); + if (!isTimerActive()){ + initISR(); + } + dShotPins |= this->_pinMask; +} + +/* + Set the throttle value and prepare the data packet and store + pin: pin of motor + throttle: 11-bit data +*/ +uint16_t DShot::setThrottle(uint8_t pin, uint16_t throttle){ + uint8_t bitMaskPin = digitalPinToBitMask(pin); + //this->_throttle[pin] = throttle; + + // TODO: This part can be further optimized when combine with create packet + this->_packet = createPacket(throttle); + uint16_t mask = 0x8000; + for (byte i=0; i<16; i++){ + if (this->_packet & mask) + dShotBits[i] |= bitMaskPin; //this->_pinMask; + else + dShotBits[i] &= bitMaskPin; //~(this->_pinMask); + mask >>= 1; + } + return _packet; +} diff --git a/src/DShot4.h b/src/DShot4.h new file mode 100644 index 0000000..d3e81a6 --- /dev/null +++ b/src/DShot4.h @@ -0,0 +1,39 @@ +#include "Arduino.h" + +#ifndef DShot_h +#define DShot_h + +#if defined(__AVR_ATmega328P__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT PORTD +#endif + +#if defined(__AVR_ATmega8__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT PORTD +// ADDON for timers +#define TIMSK1 TIMSK +#endif + +#if defined(__AVR_ATmega32U4__) +// For Leonardo, PortB 4-7: i.e. D8-D11 +#define DSHOT_PORT PORTB +#endif + +class DShot{ + public: + enum Mode { + DSHOT600, + DSHOT300, + DSHOT150 + }; + DShot(const enum Mode mode); + void attach(uint8_t pin); + uint16_t setThrottle(uint16_t throttle); + private: + uint16_t _packet = 0; + uint16_t _throttle = 0; + uint8_t _pinMask = 0; +}; + +#endif From fc9ac43689971587eaada08a8c318b45a2fabd97 Mon Sep 17 00:00:00 2001 From: Spir0u <28966599+Spir0u@users.noreply.github.com> Date: Thu, 25 Nov 2021 13:36:29 +0100 Subject: [PATCH 3/6] Modified for four ESCs --- src/DShot4.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/DShot4.cpp b/src/DShot4.cpp index 69fa210..ca641c2 100644 --- a/src/DShot4.cpp +++ b/src/DShot4.cpp @@ -1,5 +1,5 @@ #include "Arduino.h" -#include "DShot.h" +#include "DShot4.h" // Each item contains the bit of the port @@ -242,9 +242,10 @@ ISR(TIMER1_COMPA_vect){ Prepare data packet, attach 0 to telemetry bit, and calculate CRC throttle: 11-bit data */ -static inline uint16_t createPacket(uint16_t throttle){ +static inline uint16_t createPacket(uint16_t throttle, uint8_t telemetry){ uint8_t csum = 0; throttle <<= 1; // telemetry bit = 0 + throttle |= (telemetry & 0x1); // Indicate as command if less than 48 if (throttle < 48 && throttle > 0) throttle |= 1; @@ -278,12 +279,12 @@ void DShot::attach(uint8_t pin){ pin: pin of motor throttle: 11-bit data */ -uint16_t DShot::setThrottle(uint8_t pin, uint16_t throttle){ +uint16_t DShot::setThrottle(uint8_t pin, uint16_t throttle, uint8_t set_telemetry_bit){ uint8_t bitMaskPin = digitalPinToBitMask(pin); //this->_throttle[pin] = throttle; // TODO: This part can be further optimized when combine with create packet - this->_packet = createPacket(throttle); + this->_packet = createPacket(throttle, set_telemetry_bit); uint16_t mask = 0x8000; for (byte i=0; i<16; i++){ if (this->_packet & mask) From c4ba10a45a13b9fefaae87ac9b05249f2d348f7b Mon Sep 17 00:00:00 2001 From: Spir0u <28966599+Spir0u@users.noreply.github.com> Date: Thu, 25 Nov 2021 13:36:29 +0100 Subject: [PATCH 4/6] Modified for four ESCs --- examples/DShotLibraryTest/DShot.cpp | 286 +++++++++++++++++ .../DShotLibraryTest/DShot.h | 0 .../DShotLibraryTest/DShotLibraryTest.ino | 24 +- .../DShotLibraryTest_4ESCs.ino | 62 ---- examples/DShotLibraryTest_4ESCs/DShot4.cpp | 290 +++++++++++++++++ examples/DShotLibraryTest_4ESCs/DShot4.h | 37 +++ .../DShotLibraryTest_4ESCs.ino | 67 ++++ src/DShot4.cpp | 297 ------------------ 8 files changed, 692 insertions(+), 371 deletions(-) create mode 100644 examples/DShotLibraryTest/DShot.cpp rename src/DShot4.h => examples/DShotLibraryTest/DShot.h (100%) delete mode 100644 examples/DShotLibraryTest/DShotLibraryTest_4ESCs.ino create mode 100644 examples/DShotLibraryTest_4ESCs/DShot4.cpp create mode 100644 examples/DShotLibraryTest_4ESCs/DShot4.h create mode 100644 examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino delete mode 100644 src/DShot4.cpp diff --git a/examples/DShotLibraryTest/DShot.cpp b/examples/DShotLibraryTest/DShot.cpp new file mode 100644 index 0000000..b33bd10 --- /dev/null +++ b/examples/DShotLibraryTest/DShot.cpp @@ -0,0 +1,286 @@ +#include "DShot.h" +#include "Arduino.h" + +// Each item contains the bit of the port +// Pins that are not attached will always be 1 +// This is to offload the Off pattern calculation during bit send +static uint8_t dShotBits[16]; + +// Denote which pin is attached to dShot +static uint8_t dShotPins = 0; + +// Mode: 600/300/150 +static enum DShot::Mode dShotMode = DShot::Mode::DSHOT600; + +#define NOP "NOP\n" +#define NOP2 NOP NOP +#define NOP4 NOP2 NOP2 +#define NOP8 NOP4 NOP4 + +/* + DSHOT600 implementation + For 16MHz CPU, + 0: 10 cycle ON, 17 cycle OFF + 1: 20 cycle ON, 7 cycle OFF + Total 27 cycle each bit +*/ +static inline void sendData() { + noInterrupts(); + switch (dShotMode) { + case DShot::Mode::DSHOT600: + default: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_0:\n" + "OR r25, %1\n" + // Wait 7 cycles (7 - 6 = 1) + "NOP\n" + + "OUT %0, r25\n" + // Wait 10 cycles (10 - 4 = 6) + NOP4 NOP2 + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 10 cycles (10 - 2 = 8) + NOP8 + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_0\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), + "z"(dShotBits) + : "r25", "r24", "r23"); + break; + case DShot::Mode::DSHOT300: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_1:\n" + "OR r25, %1\n" + // Wait 14 cycles (14 - 6 = 8) + + // 1 + 3 * N // + "LDI r26, 2\n" // 1 // set N + "_sleep_loop_1_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + "NOP\n" + + "OUT %0, r25\n" + // Wait 20 cycles (20 - 4 = 16) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" // 2 // + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 20 cycles (20 - 2 = 18) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_1\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), + "z"(dShotBits) + : "r25", "r24", "r23"); + break; + case DShot::Mode::DSHOT150: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + + "_for_loop_2:\n" + "OR r25, %1\n" + // Wait 28 cucles (28 - 6 = 22) + + // 1 + 3 * N // + "LDI r26, 7\n" // 1 // set N + "_sleep_loop_2_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "OUT %0, r25\n" + // Wait 40 cycles (40 - 4 = 36) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 40 cycles (40 - 2 = 38) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + "NOP\n" + "NOP\n" + + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_2\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), + "z"(dShotBits) + : "r25", "r24", "r23", "r26"); + break; + } + interrupts(); +} + +static boolean timerActive = false; +/* + Generated by: + http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html + 1000 Hz Update rate +*/ +static void initISR() { + cli(); // stop interrupts + TCCR1A = 0; // set entire TCCR1A register to 0 + TCCR1B = 0; // same for TCCR1B + TCNT1 = 0; // initialize counter value to 0 + // set compare match register for 500 Hz increments + OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) + // turn on CTC mode + TCCR1B |= (1 << WGM12); + // Set CS12, CS11 and CS10 bits for 1 prescaler + TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); + // enable timer compare interrupt + TIMSK1 |= (1 << OCIE1A); + timerActive = true; + for (byte i = 0; i < 16; i++) { + dShotBits[i] = 0; + } + dShotPins = 0; + + sei(); // allow interrupts +} + +static boolean isTimerActive() { return timerActive; } + +ISR(TIMER1_COMPA_vect) { sendData(); } + +/* + Prepare data packet, attach 0 to telemetry bit, and calculate CRC + throttle: 11-bit data +*/ +static inline uint16_t createPacket(uint16_t throttle) { + uint8_t csum = 0; + throttle <<= 1; + // Indicate as command if less than 48 + if (throttle < 48 && throttle > 0) throttle |= 1; + uint16_t csum_data = throttle; + for (byte i = 0; i < 3; i++) { + csum ^= csum_data; + csum_data >>= 4; + } + csum &= 0xf; + return (throttle << 4) | csum; +} + +/****************** end of static functions *******************/ + +DShot::DShot(const enum Mode mode) { dShotMode = mode; } + +void DShot::attach(uint8_t pin) { + this->_packet = 0; + this->_pinMask = digitalPinToBitMask(pin); + pinMode(pin, OUTPUT); + if (!isTimerActive()) { + initISR(); + } + dShotPins |= this->_pinMask; +} + +/* + Set the throttle value and prepare the data packet and store + throttle: 11-bit data +*/ +uint16_t DShot::setThrottle(uint16_t throttle) { + this->_throttle = throttle; + + // TODO: This part can be further optimized when combine with create packet + this->_packet = createPacket(throttle); + uint16_t mask = 0x8000; + for (byte i = 0; i < 16; i++) { + if (this->_packet & mask) + dShotBits[i] |= this->_pinMask; + else + dShotBits[i] &= ~(this->_pinMask); + mask >>= 1; + } + return this->_packet; +} diff --git a/src/DShot4.h b/examples/DShotLibraryTest/DShot.h similarity index 100% rename from src/DShot4.h rename to examples/DShotLibraryTest/DShot.h diff --git a/examples/DShotLibraryTest/DShotLibraryTest.ino b/examples/DShotLibraryTest/DShotLibraryTest.ino index 1253af9..f1f9653 100644 --- a/examples/DShotLibraryTest/DShotLibraryTest.ino +++ b/examples/DShotLibraryTest/DShotLibraryTest.ino @@ -1,4 +1,4 @@ -#include +#include "DShot.h" /* @@ -20,31 +20,31 @@ void setup() { Serial.begin(115200); // Notice, all pins must be connected to same PORT - esc1.attach(7); + esc1.attach(7); esc1.setThrottle(throttle); } void loop() { - if (Serial.available()>0){ + if (Serial.available() > 0) { target = Serial.parseInt(); - if (target>2047) - target = 2047; + if (target > 2047) target = 2047; Serial.print(target, HEX); Serial.print("\t"); } - if (throttle<48){ + if (throttle < 48) { throttle = 48; } - if (target<=48){ + if (target <= 48) { esc1.setThrottle(target); - }else{ - if (target>throttle){ - throttle ++; + } else { + if (target > throttle) { + throttle++; esc1.setThrottle(throttle); - }else if (target - -/* - -redefine DSHOT_PORT if you want to change the default PORT - -Defaults -UNO: PORTD, available pins 0-7 (D0-D7) -Leonardo: PORTB, available pins 4-7 (D8-D11) - -e.g. -#define DSHOT_PORT PORTD -*/ - -#define M1 8 -#define M2 9 -#define M3 10 -#define M4 11 - -DShot esc(DShot::Mode::DSHOT300); - -uint16_t throttle = 0; -uint16_t target = 0; - -void setup() { - Serial.begin(115200); - - // Notice, all pins must be connected to same PORT - esc.attach(M1); - esc.setThrottle(M1, throttle); - esc.attach(M2); - esc.setThrottle(M2, throttle); - esc.attach(M3); - esc.setThrottle(M3, throttle); - esc.attach(M4); - esc.setThrottle(M4, throttle); -} - -void loop() { - if (Serial.available()>0){ - target = Serial.parseInt(); - if (target>2047) // safety measure, disarm when wrong input - target = 0; - Serial.print(target, HEX); - Serial.print("\t"); - } - if (throttle<48){ // special commands disabled - throttle = 48; - } - if (target<=48){ - esc.setThrottle(M1, target); - }else{ - if (target>throttle){ - throttle ++; - esc.setThrottle(M1, throttle); - }else if (target 0) throttle |= 1; + uint16_t csum_data = throttle; + for (byte i = 0; i < 3; i++) { + csum ^= csum_data; + csum_data >>= 4; + } + csum &= 0xf; + return (throttle << 4) | csum; +} + +/****************** end of static functions *******************/ + +DShot4::DShot4(const enum Mode mode) { dShotMode = mode; } + +void DShot4::attach(uint8_t pin) { + this->_packet = 0; + this->_pinMask = digitalPinToBitMask(pin); + pinMode(pin, OUTPUT); + if (!isTimerActive()) { + initISR(); + } + dShotPins |= this->_pinMask; +} + +/* + Set the throttle value and prepare the data packet and store + pin: pin of motor + throttle: 11-bit data +*/ +uint16_t DShot4::setThrottle(uint8_t pin, uint16_t throttle, + uint8_t set_telemetry_bit) { + uint8_t bitMaskPin = digitalPinToBitMask(pin); + // this->_throttle[pin] = throttle; + + // TODO: This part can be further optimized when combine with create packet + this->_packet = createPacket(throttle, set_telemetry_bit); + uint16_t mask = 0x8000; + for (byte i = 0; i < 16; i++) { + if (this->_packet & mask) + dShotBits[i] |= bitMaskPin; // this->_pinMask; + else + dShotBits[i] &= bitMaskPin; //~(this->_pinMask); + mask >>= 1; + } + return _packet; +} diff --git a/examples/DShotLibraryTest_4ESCs/DShot4.h b/examples/DShotLibraryTest_4ESCs/DShot4.h new file mode 100644 index 0000000..1949182 --- /dev/null +++ b/examples/DShotLibraryTest_4ESCs/DShot4.h @@ -0,0 +1,37 @@ +#include "Arduino.h" + +#ifndef DShot4_h +#define DShot4_h + +#if defined(__AVR_ATmega328P__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT PORTD +#endif + +#if defined(__AVR_ATmega8__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT PORTD +// ADDON for timers +#define TIMSK1 TIMSK +#endif + +#if defined(__AVR_ATmega32U4__) +// For Leonardo, PortB 4-7: i.e. D8-D11 +#define DSHOT_PORT PORTB +#endif + +class DShot4 { + public: + enum Mode { DSHOT600, DSHOT300, DSHOT150 }; + DShot4(const enum Mode mode); + void attach(uint8_t pin); + uint16_t DShot4::setThrottle(uint8_t pin, uint16_t throttle, + uint8_t set_telemetry_bit); + + private: + uint16_t _packet = 0; + uint16_t _throttle = 0; + uint8_t _pinMask = 0; +}; + +#endif diff --git a/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino b/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino new file mode 100644 index 0000000..b216172 --- /dev/null +++ b/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino @@ -0,0 +1,67 @@ +#include "DShot4.h" + +/* + + redefine DSHOT_PORT if you want to change the default PORT + + Defaults + UNO: PORTD, available pins 0-7 (D0-D7) + Leonardo: PORTB, available pins 4-7 (D8-D11) + + e.g. + #define DSHOT_PORT PORTD +*/ + +#define M1 8 +#define M2 9 +#define M3 10 +#define M4 11 + +DShot4 esc(DShot4::Mode::DSHOT300); + +uint16_t throttle = 0; +uint16_t target = 0; + +void setup() { + Serial.begin(115200); + + // Notice, all pins must be connected to same PORT + esc.attach(M1); + esc.setThrottle(M1, throttle, 0); + esc.attach(M2); + esc.setThrottle(M2, throttle, 0); + esc.attach(M3); + esc.setThrottle(M3, throttle, 0); + esc.attach(M4); + esc.setThrottle(M4, throttle, 0); +} + +void loop() { + if (Serial.available() > 0) { + target = Serial.parseInt(); + + if (target > 2047) // safety measure, disarm when wrong input + target = 0; + Serial.print(target, DEC); //, HEX); + Serial.print("\t"); + Serial.print(throttle, DEC); //, HEX); + Serial.print("\n"); + } + + if (throttle < 48) { // special commands disabled + throttle = 48; + } + if (target <= 48) { + esc.setThrottle(M1, target, 0); + if (target == 0) throttle = 48; + } else { + if (target > throttle) { + throttle++; + esc.setThrottle(M1, throttle, 0); + } else if (target < throttle) { + throttle--; + esc.setThrottle(M1, throttle, 0); + } + } + delay(10); +} diff --git a/src/DShot4.cpp b/src/DShot4.cpp deleted file mode 100644 index ca641c2..0000000 --- a/src/DShot4.cpp +++ /dev/null @@ -1,297 +0,0 @@ -#include "Arduino.h" -#include "DShot4.h" - - -// Each item contains the bit of the port -// Pins that are not attached will always be 1 -// This is to offload the Off pattern calculation during bit send -static uint8_t dShotBits[16]; - -// Denote which pins is attached to dShot -static uint8_t dShotPins = 0; - -// Mode: 600/300/150 -static enum DShot::Mode dShotMode = DShot::Mode::DSHOT600; - - -#define NOP "NOP\n" -#define NOP2 NOP NOP -#define NOP4 NOP2 NOP2 -#define NOP8 NOP4 NOP4 - -/* - DSHOT600 implementation - For 16MHz CPU, - 0: 10 cycle ON, 17 cycle OFF - 1: 20 cycle ON, 7 cycle OFF - Total 27 cycle each bit -*/ -static inline void sendData(){ - noInterrupts(); - switch (dShotMode) { - case DShot::Mode::DSHOT600: - default: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - "_for_loop_0:\n" - "OR r25, %1\n" - // Wait 7 cycles (7 - 6 = 1) - "NOP\n" - - "OUT %0, r25\n" - // Wait 10 cycles (10 - 4 = 6) - NOP4 - NOP2 - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 10 cycles (10 - 2 = 8) - NOP8 - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_0\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23" - ); - break; - case DShot::Mode::DSHOT300: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOTPORT |= dShotPins; - "IN r25, %0\n" - "_for_loop_1:\n" - "OR r25, %1\n" - // Wait 14 cycles (14 - 6 = 8) - - // 1 + 3 * N // - "LDI r26, 2\n" // 1 // set N - "_sleep_loop_1_1:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_1\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - "NOP\n" - - "OUT %0, r25\n" - // Wait 20 cycles (20 - 4 = 16) - - // 1 + 3 * N // - "LDI r26, 5\n" // 1 // set N - "_sleep_loop_1_2:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_2\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" // 2 // - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 20 cycles (20 - 2 = 18) - - // 1 + 3 * N // - "LDI r26, 5\n" // 1 // set N - "_sleep_loop_1_3:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_3\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_1\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23" - ); - break; - case DShot::Mode::DSHOT150: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - - "_for_loop_2:\n" - "OR r25, %1\n" - // Wait 28 cucles (28 - 6 = 22) - - // 1 + 3 * N // - "LDI r26, 7\n" // 1 // set N - "_sleep_loop_2_1:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_1\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "OUT %0, r25\n" - // Wait 40 cycles (40 - 4 = 36) - - // 1 + 3 * N // - "LDI r26, 11\n" // 1 // set N - "_sleep_loop_2_2:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_2\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 40 cycles (40 - 2 = 38) - - // 1 + 3 * N // - "LDI r26, 11\n" // 1 // set N - "_sleep_loop_2_3:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_3\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - "NOP\n" - "NOP\n" - - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_2\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23", "r26" - ); - break; - } - interrupts(); -} - -static boolean timerActive = false; - -/* - Generated by: - http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html - 1000 Hz Update rate -*/ -static void initISR(){ - cli(); // stop interrupts - TCCR1A = 0; // set entire TCCR1A register to 0 - TCCR1B = 0; // same for TCCR1B - TCNT1 = 0; // initialize counter value to 0 - // set compare match register for 500 Hz increments - OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) - // turn on CTC mode - TCCR1B |= (1 << WGM12); - // Set CS12, CS11 and CS10 bits for 1 prescaler - TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); - // enable timer compare interrupt - TIMSK1 |= (1 << OCIE1A); - timerActive = true; - for (byte i=0; i<16; i++){ - dShotBits[i] = 0; - } - dShotPins = 0; - - sei(); // allow interrupts -} - -static boolean isTimerActive(){ - return timerActive; -} - -ISR(TIMER1_COMPA_vect){ - sendData(); -} - -/* - Prepare data packet, attach 0 to telemetry bit, and calculate CRC - throttle: 11-bit data -*/ -static inline uint16_t createPacket(uint16_t throttle, uint8_t telemetry){ - uint8_t csum = 0; - throttle <<= 1; // telemetry bit = 0 - throttle |= (telemetry & 0x1); - // Indicate as command if less than 48 - if (throttle < 48 && throttle > 0) - throttle |= 1; - uint16_t csum_data = throttle; - for (byte i=0; i<3; i++){ - csum ^= csum_data; - csum_data >>= 4; - } - csum &= 0xf; - return (throttle<<4)|csum; -} - -/****************** end of static functions *******************/ - -DShot::DShot(const enum Mode mode){ - dShotMode = mode; -} - -void DShot::attach(uint8_t pin){ - this->_packet = 0; - this->_pinMask = digitalPinToBitMask(pin); - pinMode(pin, OUTPUT); - if (!isTimerActive()){ - initISR(); - } - dShotPins |= this->_pinMask; -} - -/* - Set the throttle value and prepare the data packet and store - pin: pin of motor - throttle: 11-bit data -*/ -uint16_t DShot::setThrottle(uint8_t pin, uint16_t throttle, uint8_t set_telemetry_bit){ - uint8_t bitMaskPin = digitalPinToBitMask(pin); - //this->_throttle[pin] = throttle; - - // TODO: This part can be further optimized when combine with create packet - this->_packet = createPacket(throttle, set_telemetry_bit); - uint16_t mask = 0x8000; - for (byte i=0; i<16; i++){ - if (this->_packet & mask) - dShotBits[i] |= bitMaskPin; //this->_pinMask; - else - dShotBits[i] &= bitMaskPin; //~(this->_pinMask); - mask >>= 1; - } - return _packet; -} From 574eb7bf86594423a4c52cdf5802abc47d827db8 Mon Sep 17 00:00:00 2001 From: Spir0u <28966599+Spir0u@users.noreply.github.com> Date: Thu, 25 Nov 2021 13:36:29 +0100 Subject: [PATCH 5/6] Modified for four ESCs --- examples/DShotLibraryTest/DShot.cpp | 286 +++++++++++++++++ .../DShotLibraryTest/DShot.h | 0 .../DShotLibraryTest/DShotLibraryTest.ino | 24 +- .../DShotLibraryTest_4ESCs.ino | 62 ---- examples/DShotLibraryTest_4ESCs/DShot4.cpp | 290 +++++++++++++++++ examples/DShotLibraryTest_4ESCs/DShot4.h | 37 +++ .../DShotLibraryTest_4ESCs.ino | 67 ++++ src/DShot4.cpp | 297 ------------------ 8 files changed, 692 insertions(+), 371 deletions(-) create mode 100644 examples/DShotLibraryTest/DShot.cpp rename src/DShot4.h => examples/DShotLibraryTest/DShot.h (100%) delete mode 100644 examples/DShotLibraryTest/DShotLibraryTest_4ESCs.ino create mode 100644 examples/DShotLibraryTest_4ESCs/DShot4.cpp create mode 100644 examples/DShotLibraryTest_4ESCs/DShot4.h create mode 100644 examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino delete mode 100644 src/DShot4.cpp diff --git a/examples/DShotLibraryTest/DShot.cpp b/examples/DShotLibraryTest/DShot.cpp new file mode 100644 index 0000000..b33bd10 --- /dev/null +++ b/examples/DShotLibraryTest/DShot.cpp @@ -0,0 +1,286 @@ +#include "DShot.h" +#include "Arduino.h" + +// Each item contains the bit of the port +// Pins that are not attached will always be 1 +// This is to offload the Off pattern calculation during bit send +static uint8_t dShotBits[16]; + +// Denote which pin is attached to dShot +static uint8_t dShotPins = 0; + +// Mode: 600/300/150 +static enum DShot::Mode dShotMode = DShot::Mode::DSHOT600; + +#define NOP "NOP\n" +#define NOP2 NOP NOP +#define NOP4 NOP2 NOP2 +#define NOP8 NOP4 NOP4 + +/* + DSHOT600 implementation + For 16MHz CPU, + 0: 10 cycle ON, 17 cycle OFF + 1: 20 cycle ON, 7 cycle OFF + Total 27 cycle each bit +*/ +static inline void sendData() { + noInterrupts(); + switch (dShotMode) { + case DShot::Mode::DSHOT600: + default: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_0:\n" + "OR r25, %1\n" + // Wait 7 cycles (7 - 6 = 1) + "NOP\n" + + "OUT %0, r25\n" + // Wait 10 cycles (10 - 4 = 6) + NOP4 NOP2 + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 10 cycles (10 - 2 = 8) + NOP8 + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_0\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), + "z"(dShotBits) + : "r25", "r24", "r23"); + break; + case DShot::Mode::DSHOT300: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_1:\n" + "OR r25, %1\n" + // Wait 14 cycles (14 - 6 = 8) + + // 1 + 3 * N // + "LDI r26, 2\n" // 1 // set N + "_sleep_loop_1_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + "NOP\n" + + "OUT %0, r25\n" + // Wait 20 cycles (20 - 4 = 16) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" // 2 // + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 20 cycles (20 - 2 = 18) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_1\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), + "z"(dShotBits) + : "r25", "r24", "r23"); + break; + case DShot::Mode::DSHOT150: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + + "_for_loop_2:\n" + "OR r25, %1\n" + // Wait 28 cucles (28 - 6 = 22) + + // 1 + 3 * N // + "LDI r26, 7\n" // 1 // set N + "_sleep_loop_2_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "OUT %0, r25\n" + // Wait 40 cycles (40 - 4 = 36) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 40 cycles (40 - 2 = 38) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + "NOP\n" + "NOP\n" + + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_2\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), + "z"(dShotBits) + : "r25", "r24", "r23", "r26"); + break; + } + interrupts(); +} + +static boolean timerActive = false; +/* + Generated by: + http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html + 1000 Hz Update rate +*/ +static void initISR() { + cli(); // stop interrupts + TCCR1A = 0; // set entire TCCR1A register to 0 + TCCR1B = 0; // same for TCCR1B + TCNT1 = 0; // initialize counter value to 0 + // set compare match register for 500 Hz increments + OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) + // turn on CTC mode + TCCR1B |= (1 << WGM12); + // Set CS12, CS11 and CS10 bits for 1 prescaler + TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); + // enable timer compare interrupt + TIMSK1 |= (1 << OCIE1A); + timerActive = true; + for (byte i = 0; i < 16; i++) { + dShotBits[i] = 0; + } + dShotPins = 0; + + sei(); // allow interrupts +} + +static boolean isTimerActive() { return timerActive; } + +ISR(TIMER1_COMPA_vect) { sendData(); } + +/* + Prepare data packet, attach 0 to telemetry bit, and calculate CRC + throttle: 11-bit data +*/ +static inline uint16_t createPacket(uint16_t throttle) { + uint8_t csum = 0; + throttle <<= 1; + // Indicate as command if less than 48 + if (throttle < 48 && throttle > 0) throttle |= 1; + uint16_t csum_data = throttle; + for (byte i = 0; i < 3; i++) { + csum ^= csum_data; + csum_data >>= 4; + } + csum &= 0xf; + return (throttle << 4) | csum; +} + +/****************** end of static functions *******************/ + +DShot::DShot(const enum Mode mode) { dShotMode = mode; } + +void DShot::attach(uint8_t pin) { + this->_packet = 0; + this->_pinMask = digitalPinToBitMask(pin); + pinMode(pin, OUTPUT); + if (!isTimerActive()) { + initISR(); + } + dShotPins |= this->_pinMask; +} + +/* + Set the throttle value and prepare the data packet and store + throttle: 11-bit data +*/ +uint16_t DShot::setThrottle(uint16_t throttle) { + this->_throttle = throttle; + + // TODO: This part can be further optimized when combine with create packet + this->_packet = createPacket(throttle); + uint16_t mask = 0x8000; + for (byte i = 0; i < 16; i++) { + if (this->_packet & mask) + dShotBits[i] |= this->_pinMask; + else + dShotBits[i] &= ~(this->_pinMask); + mask >>= 1; + } + return this->_packet; +} diff --git a/src/DShot4.h b/examples/DShotLibraryTest/DShot.h similarity index 100% rename from src/DShot4.h rename to examples/DShotLibraryTest/DShot.h diff --git a/examples/DShotLibraryTest/DShotLibraryTest.ino b/examples/DShotLibraryTest/DShotLibraryTest.ino index 1253af9..f1f9653 100644 --- a/examples/DShotLibraryTest/DShotLibraryTest.ino +++ b/examples/DShotLibraryTest/DShotLibraryTest.ino @@ -1,4 +1,4 @@ -#include +#include "DShot.h" /* @@ -20,31 +20,31 @@ void setup() { Serial.begin(115200); // Notice, all pins must be connected to same PORT - esc1.attach(7); + esc1.attach(7); esc1.setThrottle(throttle); } void loop() { - if (Serial.available()>0){ + if (Serial.available() > 0) { target = Serial.parseInt(); - if (target>2047) - target = 2047; + if (target > 2047) target = 2047; Serial.print(target, HEX); Serial.print("\t"); } - if (throttle<48){ + if (throttle < 48) { throttle = 48; } - if (target<=48){ + if (target <= 48) { esc1.setThrottle(target); - }else{ - if (target>throttle){ - throttle ++; + } else { + if (target > throttle) { + throttle++; esc1.setThrottle(throttle); - }else if (target - -/* - -redefine DSHOT_PORT if you want to change the default PORT - -Defaults -UNO: PORTD, available pins 0-7 (D0-D7) -Leonardo: PORTB, available pins 4-7 (D8-D11) - -e.g. -#define DSHOT_PORT PORTD -*/ - -#define M1 8 -#define M2 9 -#define M3 10 -#define M4 11 - -DShot esc(DShot::Mode::DSHOT300); - -uint16_t throttle = 0; -uint16_t target = 0; - -void setup() { - Serial.begin(115200); - - // Notice, all pins must be connected to same PORT - esc.attach(M1); - esc.setThrottle(M1, throttle); - esc.attach(M2); - esc.setThrottle(M2, throttle); - esc.attach(M3); - esc.setThrottle(M3, throttle); - esc.attach(M4); - esc.setThrottle(M4, throttle); -} - -void loop() { - if (Serial.available()>0){ - target = Serial.parseInt(); - if (target>2047) // safety measure, disarm when wrong input - target = 0; - Serial.print(target, HEX); - Serial.print("\t"); - } - if (throttle<48){ // special commands disabled - throttle = 48; - } - if (target<=48){ - esc.setThrottle(M1, target); - }else{ - if (target>throttle){ - throttle ++; - esc.setThrottle(M1, throttle); - }else if (target 0) throttle |= 1; + uint16_t csum_data = throttle; + for (byte i = 0; i < 3; i++) { + csum ^= csum_data; + csum_data >>= 4; + } + csum &= 0xf; + return (throttle << 4) | csum; +} + +/****************** end of static functions *******************/ + +DShot4::DShot4(const enum Mode mode) { dShotMode = mode; } + +void DShot4::attach(uint8_t pin) { + this->_packet = 0; + this->_pinMask = digitalPinToBitMask(pin); + pinMode(pin, OUTPUT); + if (!isTimerActive()) { + initISR(); + } + dShotPins |= this->_pinMask; +} + +/* + Set the throttle value and prepare the data packet and store + pin: pin of motor + throttle: 11-bit data +*/ +uint16_t DShot4::setThrottle(uint8_t pin, uint16_t throttle, + uint8_t set_telemetry_bit) { + uint8_t bitMaskPin = digitalPinToBitMask(pin); + // this->_throttle[pin] = throttle; + + // TODO: This part can be further optimized when combine with create packet + this->_packet = createPacket(throttle, set_telemetry_bit); + uint16_t mask = 0x8000; + for (byte i = 0; i < 16; i++) { + if (this->_packet & mask) + dShotBits[i] |= bitMaskPin; // this->_pinMask; + else + dShotBits[i] &= ~bitMaskPin; //~(this->_pinMask); + mask >>= 1; + } + return _packet; +} diff --git a/examples/DShotLibraryTest_4ESCs/DShot4.h b/examples/DShotLibraryTest_4ESCs/DShot4.h new file mode 100644 index 0000000..1949182 --- /dev/null +++ b/examples/DShotLibraryTest_4ESCs/DShot4.h @@ -0,0 +1,37 @@ +#include "Arduino.h" + +#ifndef DShot4_h +#define DShot4_h + +#if defined(__AVR_ATmega328P__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT PORTD +#endif + +#if defined(__AVR_ATmega8__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT PORTD +// ADDON for timers +#define TIMSK1 TIMSK +#endif + +#if defined(__AVR_ATmega32U4__) +// For Leonardo, PortB 4-7: i.e. D8-D11 +#define DSHOT_PORT PORTB +#endif + +class DShot4 { + public: + enum Mode { DSHOT600, DSHOT300, DSHOT150 }; + DShot4(const enum Mode mode); + void attach(uint8_t pin); + uint16_t DShot4::setThrottle(uint8_t pin, uint16_t throttle, + uint8_t set_telemetry_bit); + + private: + uint16_t _packet = 0; + uint16_t _throttle = 0; + uint8_t _pinMask = 0; +}; + +#endif diff --git a/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino b/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino new file mode 100644 index 0000000..b216172 --- /dev/null +++ b/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino @@ -0,0 +1,67 @@ +#include "DShot4.h" + +/* + + redefine DSHOT_PORT if you want to change the default PORT + + Defaults + UNO: PORTD, available pins 0-7 (D0-D7) + Leonardo: PORTB, available pins 4-7 (D8-D11) + + e.g. + #define DSHOT_PORT PORTD +*/ + +#define M1 8 +#define M2 9 +#define M3 10 +#define M4 11 + +DShot4 esc(DShot4::Mode::DSHOT300); + +uint16_t throttle = 0; +uint16_t target = 0; + +void setup() { + Serial.begin(115200); + + // Notice, all pins must be connected to same PORT + esc.attach(M1); + esc.setThrottle(M1, throttle, 0); + esc.attach(M2); + esc.setThrottle(M2, throttle, 0); + esc.attach(M3); + esc.setThrottle(M3, throttle, 0); + esc.attach(M4); + esc.setThrottle(M4, throttle, 0); +} + +void loop() { + if (Serial.available() > 0) { + target = Serial.parseInt(); + + if (target > 2047) // safety measure, disarm when wrong input + target = 0; + Serial.print(target, DEC); //, HEX); + Serial.print("\t"); + Serial.print(throttle, DEC); //, HEX); + Serial.print("\n"); + } + + if (throttle < 48) { // special commands disabled + throttle = 48; + } + if (target <= 48) { + esc.setThrottle(M1, target, 0); + if (target == 0) throttle = 48; + } else { + if (target > throttle) { + throttle++; + esc.setThrottle(M1, throttle, 0); + } else if (target < throttle) { + throttle--; + esc.setThrottle(M1, throttle, 0); + } + } + delay(10); +} diff --git a/src/DShot4.cpp b/src/DShot4.cpp deleted file mode 100644 index ca641c2..0000000 --- a/src/DShot4.cpp +++ /dev/null @@ -1,297 +0,0 @@ -#include "Arduino.h" -#include "DShot4.h" - - -// Each item contains the bit of the port -// Pins that are not attached will always be 1 -// This is to offload the Off pattern calculation during bit send -static uint8_t dShotBits[16]; - -// Denote which pins is attached to dShot -static uint8_t dShotPins = 0; - -// Mode: 600/300/150 -static enum DShot::Mode dShotMode = DShot::Mode::DSHOT600; - - -#define NOP "NOP\n" -#define NOP2 NOP NOP -#define NOP4 NOP2 NOP2 -#define NOP8 NOP4 NOP4 - -/* - DSHOT600 implementation - For 16MHz CPU, - 0: 10 cycle ON, 17 cycle OFF - 1: 20 cycle ON, 7 cycle OFF - Total 27 cycle each bit -*/ -static inline void sendData(){ - noInterrupts(); - switch (dShotMode) { - case DShot::Mode::DSHOT600: - default: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - "_for_loop_0:\n" - "OR r25, %1\n" - // Wait 7 cycles (7 - 6 = 1) - "NOP\n" - - "OUT %0, r25\n" - // Wait 10 cycles (10 - 4 = 6) - NOP4 - NOP2 - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 10 cycles (10 - 2 = 8) - NOP8 - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_0\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23" - ); - break; - case DShot::Mode::DSHOT300: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOTPORT |= dShotPins; - "IN r25, %0\n" - "_for_loop_1:\n" - "OR r25, %1\n" - // Wait 14 cycles (14 - 6 = 8) - - // 1 + 3 * N // - "LDI r26, 2\n" // 1 // set N - "_sleep_loop_1_1:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_1\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - "NOP\n" - - "OUT %0, r25\n" - // Wait 20 cycles (20 - 4 = 16) - - // 1 + 3 * N // - "LDI r26, 5\n" // 1 // set N - "_sleep_loop_1_2:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_2\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" // 2 // - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 20 cycles (20 - 2 = 18) - - // 1 + 3 * N // - "LDI r26, 5\n" // 1 // set N - "_sleep_loop_1_3:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_3\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_1\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23" - ); - break; - case DShot::Mode::DSHOT150: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - - "_for_loop_2:\n" - "OR r25, %1\n" - // Wait 28 cucles (28 - 6 = 22) - - // 1 + 3 * N // - "LDI r26, 7\n" // 1 // set N - "_sleep_loop_2_1:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_1\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "OUT %0, r25\n" - // Wait 40 cycles (40 - 4 = 36) - - // 1 + 3 * N // - "LDI r26, 11\n" // 1 // set N - "_sleep_loop_2_2:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_2\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 40 cycles (40 - 2 = 38) - - // 1 + 3 * N // - "LDI r26, 11\n" // 1 // set N - "_sleep_loop_2_3:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_3\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - "NOP\n" - "NOP\n" - - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_2\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23", "r26" - ); - break; - } - interrupts(); -} - -static boolean timerActive = false; - -/* - Generated by: - http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html - 1000 Hz Update rate -*/ -static void initISR(){ - cli(); // stop interrupts - TCCR1A = 0; // set entire TCCR1A register to 0 - TCCR1B = 0; // same for TCCR1B - TCNT1 = 0; // initialize counter value to 0 - // set compare match register for 500 Hz increments - OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) - // turn on CTC mode - TCCR1B |= (1 << WGM12); - // Set CS12, CS11 and CS10 bits for 1 prescaler - TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); - // enable timer compare interrupt - TIMSK1 |= (1 << OCIE1A); - timerActive = true; - for (byte i=0; i<16; i++){ - dShotBits[i] = 0; - } - dShotPins = 0; - - sei(); // allow interrupts -} - -static boolean isTimerActive(){ - return timerActive; -} - -ISR(TIMER1_COMPA_vect){ - sendData(); -} - -/* - Prepare data packet, attach 0 to telemetry bit, and calculate CRC - throttle: 11-bit data -*/ -static inline uint16_t createPacket(uint16_t throttle, uint8_t telemetry){ - uint8_t csum = 0; - throttle <<= 1; // telemetry bit = 0 - throttle |= (telemetry & 0x1); - // Indicate as command if less than 48 - if (throttle < 48 && throttle > 0) - throttle |= 1; - uint16_t csum_data = throttle; - for (byte i=0; i<3; i++){ - csum ^= csum_data; - csum_data >>= 4; - } - csum &= 0xf; - return (throttle<<4)|csum; -} - -/****************** end of static functions *******************/ - -DShot::DShot(const enum Mode mode){ - dShotMode = mode; -} - -void DShot::attach(uint8_t pin){ - this->_packet = 0; - this->_pinMask = digitalPinToBitMask(pin); - pinMode(pin, OUTPUT); - if (!isTimerActive()){ - initISR(); - } - dShotPins |= this->_pinMask; -} - -/* - Set the throttle value and prepare the data packet and store - pin: pin of motor - throttle: 11-bit data -*/ -uint16_t DShot::setThrottle(uint8_t pin, uint16_t throttle, uint8_t set_telemetry_bit){ - uint8_t bitMaskPin = digitalPinToBitMask(pin); - //this->_throttle[pin] = throttle; - - // TODO: This part can be further optimized when combine with create packet - this->_packet = createPacket(throttle, set_telemetry_bit); - uint16_t mask = 0x8000; - for (byte i=0; i<16; i++){ - if (this->_packet & mask) - dShotBits[i] |= bitMaskPin; //this->_pinMask; - else - dShotBits[i] &= bitMaskPin; //~(this->_pinMask); - mask >>= 1; - } - return _packet; -} From 4c5d01aaf173e69ff7223e52aaf3b61c817aa98f Mon Sep 17 00:00:00 2001 From: Spir0u <28966599+Spir0u@users.noreply.github.com> Date: Fri, 8 Jul 2022 01:22:45 +0200 Subject: [PATCH 6/6] Updated version with working 8 ESCs. Telemetry is available for one ESC, but the serial output was not tested. --- README.md | 7 +- examples/DShotLibraryTest_4ESCs/DShot.cpp | 621 ++++++++++++++ examples/DShotLibraryTest_4ESCs/DShot.h | 52 ++ examples/DShotLibraryTest_4ESCs/DShot4.cpp | 290 ------- examples/DShotLibraryTest_4ESCs/DShot4.h | 37 - .../DShotLibraryTest_4ESCs.ino | 8 +- examples/DShotLibraryTest_4ESCs/queue.cpp | 10 + examples/DShotLibraryTest_8ESCs/DShot.cpp | 621 ++++++++++++++ examples/DShotLibraryTest_8ESCs/DShot.h | 52 ++ .../DShotLibraryTest_8ESCs/DShotKeywords.h | 115 +++ .../DShotLibraryTest_8ESCs.ino | 283 +++++++ examples/DShotLibraryTest_8ESCs/queue.cpp | 10 + examples/telemetry_example/DShot.ino | 430 ++++++++++ src/DShot.cpp | 796 +++++++++++++----- src/DShot.h | 47 +- src/DShotKeywords.h | 115 +++ src/Dshot_Digital_Cmd_Spec.txt | 78 -- src/queue.cpp | 9 + 18 files changed, 2920 insertions(+), 661 deletions(-) create mode 100644 examples/DShotLibraryTest_4ESCs/DShot.cpp create mode 100644 examples/DShotLibraryTest_4ESCs/DShot.h delete mode 100644 examples/DShotLibraryTest_4ESCs/DShot4.cpp delete mode 100644 examples/DShotLibraryTest_4ESCs/DShot4.h create mode 100644 examples/DShotLibraryTest_4ESCs/queue.cpp create mode 100644 examples/DShotLibraryTest_8ESCs/DShot.cpp create mode 100644 examples/DShotLibraryTest_8ESCs/DShot.h create mode 100644 examples/DShotLibraryTest_8ESCs/DShotKeywords.h create mode 100644 examples/DShotLibraryTest_8ESCs/DShotLibraryTest_8ESCs.ino create mode 100644 examples/DShotLibraryTest_8ESCs/queue.cpp create mode 100644 examples/telemetry_example/DShot.ino create mode 100644 src/DShotKeywords.h delete mode 100644 src/Dshot_Digital_Cmd_Spec.txt create mode 100644 src/queue.cpp diff --git a/README.md b/README.md index 1f74248..439202f 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,9 @@ # DShot-Arduino DShot implementation for Arduino using bit-banging method -Test with four ESCs \ No newline at end of file +Used with up to eight ESCs with an Arduino Leonardo + +Needs the Queue Arduino Library from https://github.com/SMFSW/Queue + +In the DShotLibraryTest is the code from the original repo, then there is a code example for four and eight ESCs. +In the telemetry_example is the arduino code used in a project as a reference. \ No newline at end of file diff --git a/examples/DShotLibraryTest_4ESCs/DShot.cpp b/examples/DShotLibraryTest_4ESCs/DShot.cpp new file mode 100644 index 0000000..ad12446 --- /dev/null +++ b/examples/DShotLibraryTest_4ESCs/DShot.cpp @@ -0,0 +1,621 @@ +#include "DShot.h" + +// TODO: See if timing works for sequence of telemetry requests: DSHOT600: t_period = x us, +// t_telemetry_packet = x ms? Telemetry sent back over one UART --> one: first port has priority +#define MAX_TELEMETRY_REQUESTS 8 +#define TELEMETRY_WAIT 0 // 4* dShotMode + // TELEMETRY_SEND_TIME/DSHOT150_PERIOD * (dShotMode+1) --> since the dshot + // commands are sent at 500 Hz rate, no wait is required. +//------------------------------------------------ + +#define DSHOT150_PERIOD 247 // us +#define TELEMETRY_SEND_TIME 900 // us + +// Denote which pins are attached to dShot +static uint8_t dShotPins[] = {0, 0}; +// Each item contains the bit of the port +// Pins that are not attached will always be 1 +// This is to offload the Off pattern calculation during bit send +static uint8_t dShotBits[2][16]; +// counter, how many telemetry requests have to be done. first come first served. +static uint8_t telemetryRequests[] = {0, 0}; + +static uint8_t tlm_req_count = 0; + +static uint8_t dShotBitsTemp[16] = {0}; + +static Queue dShotBitsT[2]; // FIFO Queue for telemetry bits +static cppQueue telemetryRequestsPort(sizeof(uint8_t), 2 * MAX_TELEMETRY_REQUESTS, + FIFO); // FIFO Queue for telemetry port + +// counter to set to the wait time in DSHOTperiod units +static uint16_t telemetryRequestWait = 0; + +// Mode: 600/300/150 +static enum DShot::Mode dShotMode = DShot::Mode::DSHOT600; + +#define NOP "NOP\n" +#define NOP2 NOP NOP +#define NOP4 NOP2 NOP2 +#define NOP8 NOP4 NOP4 + +/* + DSHOT600 implementation + For 16MHz CPU, + 0: 10 cycle ON, 17 cycle OFF + 1: 20 cycle ON, 7 cycle OFF + Total 27 cycle each bit +*/ +static inline void sendData() { + noInterrupts(); + + // Logic for sending one telemetry request at a time + uint8_t dShotBitsSend[2][16] = {0}; +#ifndef DEBUG + if (telemetryRequestWait == 0) { + if (telemetryRequestsPort.getCount() > 0) { + uint8_t port; + telemetryRequestsPort.pop(&port); + if (port == 0) { + // if (telemetryRequests[0] > 0) { + dShotBitsT[0].pop(dShotBitsSend[0]); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + --telemetryRequests[0]; + // } else if (telemetryRequests[1] > 0) { + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + dShotBitsT[1].pop(dShotBitsSend[1]); + --telemetryRequests[1]; + } + telemetryRequestWait = TELEMETRY_WAIT; // for debug + --tlm_req_count; + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + } else { + telemetryRequestWait--; + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } +#endif + + switch (dShotMode) { + case DShot::Mode::DSHOT600: + default: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_0:\n" + "OR r25, %1\n" + // Wait 7 cycles (7 - 6 = 1) + "NOP\n" + + "OUT %0, r25\n" + // Wait 10 cycles (10 - 4 = 6) + NOP4 NOP2 + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 10 cycles (10 - 2 = 8) + NOP8 + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_0\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_02:\n" + "OR r25, %1\n" + // Wait 7 cycles (7 - 6 = 1) + "NOP\n" + + "OUT %0, r25\n" + // Wait 10 cycles (10 - 4 = 6) + NOP4 NOP2 + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 10 cycles (10 - 2 = 8) + NOP8 + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_02\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23"); +#endif + break; + case DShot::Mode::DSHOT300: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOTPORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_1:\n" + "OR r25, %1\n" + // Wait 14 cycles (14 - 6 = 8) + + // 1 + 3 * N // + "LDI r26, 2\n" // 1 // set N + "_sleep_loop_1_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + "NOP\n" + + "OUT %0, r25\n" + // Wait 20 cycles (20 - 4 = 16) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" // 2 // + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 20 cycles (20 - 2 = 18) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_1\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOTPORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_12:\n" + "OR r25, %1\n" + // Wait 14 cycles (14 - 6 = 8) + + // 1 + 3 * N // + "LDI r26, 2\n" // 1 // set N + "_sleep_loop_12_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + "NOP\n" + + "OUT %0, r25\n" + // Wait 20 cycles (20 - 4 = 16) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_12_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" // 2 // + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 20 cycles (20 - 2 = 18) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_12_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_12\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23"); +#endif + break; + case DShot::Mode::DSHOT150: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + + "_for_loop_2:\n" + "OR r25, %1\n" + // Wait 28 cucles (28 - 6 = 22) + + // 1 + 3 * N // + "LDI r26, 7\n" // 1 // set N + "_sleep_loop_2_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "OUT %0, r25\n" + // Wait 40 cycles (40 - 4 = 36) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 40 cycles (40 - 2 = 38) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + "NOP\n" + "NOP\n" + + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_2\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23", "r26"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + + "_for_loop_22:\n" + "OR r25, %1\n" + // Wait 28 cucles (28 - 6 = 22) + + // 1 + 3 * N // + "LDI r26, 7\n" // 1 // set N + "_sleep_loop_22_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "OUT %0, r25\n" + // Wait 40 cycles (40 - 4 = 36) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_22_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 40 cycles (40 - 2 = 38) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_22_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + "NOP\n" + "NOP\n" + + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_22\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23", "r26"); +#endif + + break; + } + interrupts(); +} + +static boolean timerActive = false; + +/* + Generated by: + http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html + 500 Hz Update rate +*/ +static void initISR() { + cli(); // stop interrupts + TCCR1A = 0; // set entire TCCR1A register to 0 + TCCR1B = 0; // same for TCCR1B + TCNT1 = 0; // initialize counter value to 0 + // set compare match register for 500 Hz increments + OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) + // turn on CTC mode + TCCR1B |= (1 << WGM12); + // Set CS12, CS11 and CS10 bits for 1 prescaler + TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); + // enable timer compare interrupt + TIMSK1 |= (1 << OCIE1A); + timerActive = true; + for (uint8_t p = 0; p < 2; p++) { + for (uint8_t t = 0; t < MAX_TELEMETRY_REQUESTS; t++) { + for (uint8_t i = 0; i < 16; i++) { + // dShotBits[p][t][i] = 0; + } + } + dShotPins[p] = 0; + } + + sei(); // allow interrupts +} + +static boolean isTimerActive() { return timerActive; } + +ISR(TIMER1_COMPA_vect) { sendData(); } + +/* + Prepare data packet, attach 0 to telemetry bit, and calculate CRC + throttle: 11-bit data +*/ +static inline uint16_t createPacket(uint16_t throttle, bool telemetry) { + uint8_t csum = 0; + throttle <<= 1; + throttle |= telemetry; + uint16_t csum_data = throttle; + for (byte i = 0; i < 3; i++) { + csum ^= csum_data; + csum_data >>= 4; + } + csum &= 0xf; + return (throttle << 4) | csum; +} + +/****************** end of static functions *******************/ + +DShot::DShot(const enum Mode mode) { dShotMode = mode; } + +void DShot::attach(uint8_t pin) { + uint8_t pinMask = digitalPinToBitMask(pin); + uint8_t p = (digitalPinToPort(pin) == PORT1 ? 0 : 1); + pinMode(pin, OUTPUT); + if (!isTimerActive()) { + initISR(); + } + dShotPins[p] |= pinMask; +} + +/* + Set the throttle value and prepare the data packet and store + pin: pin of motor + throttle: 11-bit data +*/ +uint16_t DShot::setThrottle(uint8_t pin, uint16_t throttle, bool set_telemetry_bit) { + uint8_t bitMaskPin = digitalPinToBitMask(pin); + uint8_t p = (digitalPinToPort(pin) == PORT1 ? 0 : 1); + uint8_t dShotBitsTemp[16] = {0}; + memcpy(dShotBitsTemp, dShotBits[p], sizeof(dShotBitsTemp)); + + uint16_t packet = createPacket(throttle, 0); + uint16_t mask = 0x8000; + for (byte i = 0; i < 16; i++) { + if (packet & mask) + dShotBitsTemp[i] |= bitMaskPin; + else + dShotBitsTemp[i] &= ~bitMaskPin; + mask >>= 1; + } + memcpy(dShotBits[p], dShotBitsTemp, sizeof(dShotBitsTemp)); + + // do not request telemetry if there are already all slots reserved + if (set_telemetry_bit && dShotBitsT[p].getCount() < MAX_TELEMETRY_REQUESTS) { + dShotBitsTemp[11] |= bitMaskPin; // telemetry request bit + dShotBitsTemp[15] ^= bitMaskPin; // crc checksum + ++telemetryRequests[p]; + ++tlm_req_count; + + noInterrupts(); // to block unwanted states + telemetryRequestsPort.push(&p); + dShotBitsT[p].push(&dShotBitsTemp); + interrupts(); + } + +#ifdef DEBUG + DEBUG DEFINED ! // here to give compile error + uint8_t dShotBitsSend[2][16] = {0}; + if (telemetryRequestWait == 0) { + if (telemetryRequestsPort.getCount() > 0) { + uint8_t port; + telemetryRequestsPort.pop(&port); + p = port; + uint8_t dShotBitsTemp[16] = {0}; + if (port == 0) { + dShotBitsT[0].pop(&dShotBitsTemp); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + --telemetryRequests[0]; + memcpy(dShotBitsSend[0], dShotBitsTemp, sizeof(dShotBitsTemp)); + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + dShotBitsT[1].pop(&dShotBitsTemp); + memcpy(dShotBitsSend[1], dShotBitsTemp, sizeof(dShotBitsTemp)); + --telemetryRequests[1]; + } + telemetryRequestWait = TELEMETRY_WAIT; + --tlm_req_count; + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + } else { + telemetryRequestWait--; + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + return dShotBitsSend[p][11] + 200; +#endif + return 0; + // for debugging + return (dShotBitsT[0].getCount() * 10 + dShotBitsT[1].getCount()) * 100 + + telemetryRequestsPort.getCount(); +} + +/* + Send the ESC command immediatly, send motor speed 0 (command 48) afterwards + pin: pin of motor + command: 0-48, see Dshot_Digital_Cmd_Spec.txt +*/ +uint16_t DShot::sendCommand(uint8_t pin, uint16_t command) { + uint16_t sleepTime = 0; + noInterrupts(); + switch (command) { + case 1 ... 2: + sleepTime = 260; + setThrottle(pin, command, 0); + sendData(); + break; + case 3 ... 4: + sleepTime = 280; + setThrottle(pin, command, 0); + sendData(); + break; + case 5: + sleepTime = 1020; + setThrottle(pin, command, 0); + sendData(); + break; + case 6: + sleepTime = 12; + setThrottle(pin, command, 0); + sendData(); + break; + case 12: + sleepTime = 35; + case 7 ... 10: + case 13 ... 21: + case 32 ... 35: + setThrottle(pin, command, 0); + sendData(); // not tested yet + sendData(); + sendData(); + sendData(); + sendData(); + sendData(); + break; + default: // sends also not implemented commands once + setThrottle(pin, command, 0); + } + delay(sleepTime); + setThrottle(pin, 48, 0); // send motor speed 0 command + interrupts(); +} diff --git a/examples/DShotLibraryTest_4ESCs/DShot.h b/examples/DShotLibraryTest_4ESCs/DShot.h new file mode 100644 index 0000000..1bf10e5 --- /dev/null +++ b/examples/DShotLibraryTest_4ESCs/DShot.h @@ -0,0 +1,52 @@ +#include "Arduino.h" +#include // Arduino Library from https://github.com/SMFSW/Queue +#include "queue.cpp" + +#ifndef DShot_h +#define DShot_h + +// #define DEBUG + +#if defined(__AVR_ATmega328P__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT1 PORTD +#define PORT1 digitalPinToPort(0) // Arduino Port number for PORTD +#endif + +#if defined(__AVR_ATmega8__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT1 PORTD +#define PORT1 digitalPinToPort(0) // Arduino Port number for PORTD +// ADDON for timers +#define TIMSK1 TIMSK +#endif + +#if defined(__AVR_ATmega32U4__) +// For Leonardo, PortB 4-7: i.e. D8-D11 +#define DSHOT_PORT1 PORTB +#define PORT1 digitalPinToPort(8) // Arduino Port number for PORTB +// More pins: PortF 18-21: i.e. D18-D21 +// #define DSHOT_PORT2 PORTF +// #define PORT2 digitalPinToPort(18) // Arduino Port number for PORTF +#endif + +class DShot { + public: + enum Mode { DSHOT600 = 4, DSHOT300 = 2, DSHOT150 = 1 }; + DShot(const enum Mode mode); + + // Initialises the output pin. Call once before sending anything + void attach(uint8_t pin); + + // Send throttle value. If set_telemetry_bit is true, it will request telemetry once, and + // afterwards sends the throttle value without telemetry request. + uint16_t setThrottle(uint8_t pin, uint16_t throttle, bool set_telemetry_bit); + + // Send commands from List as many times as needed, waiting the specified time before sending 48 + // command (0 speed) + uint16_t sendCommand(uint8_t pin, uint16_t command); + + private: +}; + +#endif diff --git a/examples/DShotLibraryTest_4ESCs/DShot4.cpp b/examples/DShotLibraryTest_4ESCs/DShot4.cpp deleted file mode 100644 index 613c295..0000000 --- a/examples/DShotLibraryTest_4ESCs/DShot4.cpp +++ /dev/null @@ -1,290 +0,0 @@ -#include "DShot4.h" -#include "Arduino.h" - -// Denote which pins is attached to dShot -static uint8_t dShotPins = 0; -// Each item contains the bit of the port -// Pins that are not attached will always be 1 -// This is to offload the Off pattern calculation during bit send -static uint8_t dShotBits[16]; - -// Mode: 600/300/150 -static enum DShot4::Mode dShotMode = DShot4::Mode::DSHOT600; - -#define NOP "NOP\n" -#define NOP2 NOP NOP -#define NOP4 NOP2 NOP2 -#define NOP8 NOP4 NOP4 - -/* - DSHOT600 implementation - For 16MHz CPU, - 0: 10 cycle ON, 17 cycle OFF - 1: 20 cycle ON, 7 cycle OFF - Total 27 cycle each bit -*/ -static inline void sendData() { - noInterrupts(); - switch (dShotMode) { - case DShot4::Mode::DSHOT600: - default: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - "_for_loop_0:\n" - "OR r25, %1\n" - // Wait 7 cycles (7 - 6 = 1) - "NOP\n" - - "OUT %0, r25\n" - // Wait 10 cycles (10 - 4 = 6) - NOP4 NOP2 - // Set Low for low bits only - // DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 10 cycles (10 - 2 = 8) - NOP8 - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_0\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no - // wait - : - : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), - "z"(dShotBits) - : "r25", "r24", "r23"); - break; - case DShot4::Mode::DSHOT300: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOTPORT |= dShotPins; - "IN r25, %0\n" - "_for_loop_1:\n" - "OR r25, %1\n" - // Wait 14 cycles (14 - 6 = 8) - - // 1 + 3 * N // - "LDI r26, 2\n" // 1 // set N - "_sleep_loop_1_1:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_1\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - "NOP\n" - - "OUT %0, r25\n" - // Wait 20 cycles (20 - 4 = 16) - - // 1 + 3 * N // - "LDI r26, 5\n" // 1 // set N - "_sleep_loop_1_2:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_2\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - // Set Low for low bits only - // DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" // 2 // - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 20 cycles (20 - 2 = 18) - - // 1 + 3 * N // - "LDI r26, 5\n" // 1 // set N - "_sleep_loop_1_3:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_3\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_1\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no - // wait - : - : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), - "z"(dShotBits) - : "r25", "r24", "r23"); - break; - case DShot4::Mode::DSHOT150: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - - "_for_loop_2:\n" - "OR r25, %1\n" - // Wait 28 cucles (28 - 6 = 22) - - // 1 + 3 * N // - "LDI r26, 7\n" // 1 // set N - "_sleep_loop_2_1:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_1\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "OUT %0, r25\n" - // Wait 40 cycles (40 - 4 = 36) - - // 1 + 3 * N // - "LDI r26, 11\n" // 1 // set N - "_sleep_loop_2_2:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_2\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - - // Set Low for low bits only - // DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 40 cycles (40 - 2 = 38) - - // 1 + 3 * N // - "LDI r26, 11\n" // 1 // set N - "_sleep_loop_2_3:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_3\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - "NOP\n" - "NOP\n" - - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_2\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no - // wait - : - : "I"(_SFR_IO_ADDR(DSHOT_PORT)), "r"(dShotPins), "r"(~dShotPins), - "z"(dShotBits) - : "r25", "r24", "r23", "r26"); - break; - } - interrupts(); -} - -static boolean timerActive = false; - -/* - Generated by: - http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html - 1000 Hz Update rate -*/ -static void initISR() { - cli(); // stop interrupts - TCCR1A = 0; // set entire TCCR1A register to 0 - TCCR1B = 0; // same for TCCR1B - TCNT1 = 0; // initialize counter value to 0 - // set compare match register for 500 Hz increments - OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) - // turn on CTC mode - TCCR1B |= (1 << WGM12); - // Set CS12, CS11 and CS10 bits for 1 prescaler - TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); - // enable timer compare interrupt - TIMSK1 |= (1 << OCIE1A); - timerActive = true; - for (byte i = 0; i < 16; i++) { - dShotBits[i] = 0; - } - dShotPins = 0; - - sei(); // allow interrupts -} - -static boolean isTimerActive() { return timerActive; } - -ISR(TIMER1_COMPA_vect) { sendData(); } - -/* - Prepare data packet, attach 0 to telemetry bit, and calculate CRC - throttle: 11-bit data -*/ -static inline uint16_t createPacket(uint16_t throttle, uint8_t telemetry) { - uint8_t csum = 0; - throttle <<= 1; // telemetry bit = 0 - throttle |= (telemetry & 0x1); - // Indicate as command if less than 48 - if (throttle < 48 && throttle > 0) throttle |= 1; - uint16_t csum_data = throttle; - for (byte i = 0; i < 3; i++) { - csum ^= csum_data; - csum_data >>= 4; - } - csum &= 0xf; - return (throttle << 4) | csum; -} - -/****************** end of static functions *******************/ - -DShot4::DShot4(const enum Mode mode) { dShotMode = mode; } - -void DShot4::attach(uint8_t pin) { - this->_packet = 0; - this->_pinMask = digitalPinToBitMask(pin); - pinMode(pin, OUTPUT); - if (!isTimerActive()) { - initISR(); - } - dShotPins |= this->_pinMask; -} - -/* - Set the throttle value and prepare the data packet and store - pin: pin of motor - throttle: 11-bit data -*/ -uint16_t DShot4::setThrottle(uint8_t pin, uint16_t throttle, - uint8_t set_telemetry_bit) { - uint8_t bitMaskPin = digitalPinToBitMask(pin); - // this->_throttle[pin] = throttle; - - // TODO: This part can be further optimized when combine with create packet - this->_packet = createPacket(throttle, set_telemetry_bit); - uint16_t mask = 0x8000; - for (byte i = 0; i < 16; i++) { - if (this->_packet & mask) - dShotBits[i] |= bitMaskPin; // this->_pinMask; - else - dShotBits[i] &= ~bitMaskPin; //~(this->_pinMask); - mask >>= 1; - } - return _packet; -} diff --git a/examples/DShotLibraryTest_4ESCs/DShot4.h b/examples/DShotLibraryTest_4ESCs/DShot4.h deleted file mode 100644 index 1949182..0000000 --- a/examples/DShotLibraryTest_4ESCs/DShot4.h +++ /dev/null @@ -1,37 +0,0 @@ -#include "Arduino.h" - -#ifndef DShot4_h -#define DShot4_h - -#if defined(__AVR_ATmega328P__) -// For UNO, PortD 0-7: i.e. D0-D7 -#define DSHOT_PORT PORTD -#endif - -#if defined(__AVR_ATmega8__) -// For UNO, PortD 0-7: i.e. D0-D7 -#define DSHOT_PORT PORTD -// ADDON for timers -#define TIMSK1 TIMSK -#endif - -#if defined(__AVR_ATmega32U4__) -// For Leonardo, PortB 4-7: i.e. D8-D11 -#define DSHOT_PORT PORTB -#endif - -class DShot4 { - public: - enum Mode { DSHOT600, DSHOT300, DSHOT150 }; - DShot4(const enum Mode mode); - void attach(uint8_t pin); - uint16_t DShot4::setThrottle(uint8_t pin, uint16_t throttle, - uint8_t set_telemetry_bit); - - private: - uint16_t _packet = 0; - uint16_t _throttle = 0; - uint8_t _pinMask = 0; -}; - -#endif diff --git a/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino b/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino index b216172..c5cd580 100644 --- a/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino +++ b/examples/DShotLibraryTest_4ESCs/DShotLibraryTest_4ESCs.ino @@ -1,4 +1,4 @@ -#include "DShot4.h" +#include "DShot.h" /* @@ -17,7 +17,7 @@ #define M3 10 #define M4 11 -DShot4 esc(DShot4::Mode::DSHOT300); +DShot esc(DShot::Mode::DSHOT300); uint16_t throttle = 0; uint16_t target = 0; @@ -42,9 +42,9 @@ void loop() { if (target > 2047) // safety measure, disarm when wrong input target = 0; - Serial.print(target, DEC); //, HEX); + Serial.print(target, DEC); Serial.print("\t"); - Serial.print(throttle, DEC); //, HEX); + Serial.print(throttle, DEC); Serial.print("\n"); } diff --git a/examples/DShotLibraryTest_4ESCs/queue.cpp b/examples/DShotLibraryTest_4ESCs/queue.cpp new file mode 100644 index 0000000..5ef6759 --- /dev/null +++ b/examples/DShotLibraryTest_4ESCs/queue.cpp @@ -0,0 +1,10 @@ + +#include + +#define IMPLEMENTATION FIFO +#define MAX_TELEMETRY_REQUESTS 8 + +class Queue : public cppQueue { + public: + Queue() : cppQueue(sizeof(uint8_t[16]), MAX_TELEMETRY_REQUESTS, IMPLEMENTATION){}; +}; diff --git a/examples/DShotLibraryTest_8ESCs/DShot.cpp b/examples/DShotLibraryTest_8ESCs/DShot.cpp new file mode 100644 index 0000000..ad12446 --- /dev/null +++ b/examples/DShotLibraryTest_8ESCs/DShot.cpp @@ -0,0 +1,621 @@ +#include "DShot.h" + +// TODO: See if timing works for sequence of telemetry requests: DSHOT600: t_period = x us, +// t_telemetry_packet = x ms? Telemetry sent back over one UART --> one: first port has priority +#define MAX_TELEMETRY_REQUESTS 8 +#define TELEMETRY_WAIT 0 // 4* dShotMode + // TELEMETRY_SEND_TIME/DSHOT150_PERIOD * (dShotMode+1) --> since the dshot + // commands are sent at 500 Hz rate, no wait is required. +//------------------------------------------------ + +#define DSHOT150_PERIOD 247 // us +#define TELEMETRY_SEND_TIME 900 // us + +// Denote which pins are attached to dShot +static uint8_t dShotPins[] = {0, 0}; +// Each item contains the bit of the port +// Pins that are not attached will always be 1 +// This is to offload the Off pattern calculation during bit send +static uint8_t dShotBits[2][16]; +// counter, how many telemetry requests have to be done. first come first served. +static uint8_t telemetryRequests[] = {0, 0}; + +static uint8_t tlm_req_count = 0; + +static uint8_t dShotBitsTemp[16] = {0}; + +static Queue dShotBitsT[2]; // FIFO Queue for telemetry bits +static cppQueue telemetryRequestsPort(sizeof(uint8_t), 2 * MAX_TELEMETRY_REQUESTS, + FIFO); // FIFO Queue for telemetry port + +// counter to set to the wait time in DSHOTperiod units +static uint16_t telemetryRequestWait = 0; + +// Mode: 600/300/150 +static enum DShot::Mode dShotMode = DShot::Mode::DSHOT600; + +#define NOP "NOP\n" +#define NOP2 NOP NOP +#define NOP4 NOP2 NOP2 +#define NOP8 NOP4 NOP4 + +/* + DSHOT600 implementation + For 16MHz CPU, + 0: 10 cycle ON, 17 cycle OFF + 1: 20 cycle ON, 7 cycle OFF + Total 27 cycle each bit +*/ +static inline void sendData() { + noInterrupts(); + + // Logic for sending one telemetry request at a time + uint8_t dShotBitsSend[2][16] = {0}; +#ifndef DEBUG + if (telemetryRequestWait == 0) { + if (telemetryRequestsPort.getCount() > 0) { + uint8_t port; + telemetryRequestsPort.pop(&port); + if (port == 0) { + // if (telemetryRequests[0] > 0) { + dShotBitsT[0].pop(dShotBitsSend[0]); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + --telemetryRequests[0]; + // } else if (telemetryRequests[1] > 0) { + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + dShotBitsT[1].pop(dShotBitsSend[1]); + --telemetryRequests[1]; + } + telemetryRequestWait = TELEMETRY_WAIT; // for debug + --tlm_req_count; + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + } else { + telemetryRequestWait--; + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } +#endif + + switch (dShotMode) { + case DShot::Mode::DSHOT600: + default: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_0:\n" + "OR r25, %1\n" + // Wait 7 cycles (7 - 6 = 1) + "NOP\n" + + "OUT %0, r25\n" + // Wait 10 cycles (10 - 4 = 6) + NOP4 NOP2 + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 10 cycles (10 - 2 = 8) + NOP8 + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_0\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_02:\n" + "OR r25, %1\n" + // Wait 7 cycles (7 - 6 = 1) + "NOP\n" + + "OUT %0, r25\n" + // Wait 10 cycles (10 - 4 = 6) + NOP4 NOP2 + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 10 cycles (10 - 2 = 8) + NOP8 + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_02\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23"); +#endif + break; + case DShot::Mode::DSHOT300: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOTPORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_1:\n" + "OR r25, %1\n" + // Wait 14 cycles (14 - 6 = 8) + + // 1 + 3 * N // + "LDI r26, 2\n" // 1 // set N + "_sleep_loop_1_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + "NOP\n" + + "OUT %0, r25\n" + // Wait 20 cycles (20 - 4 = 16) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" // 2 // + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 20 cycles (20 - 2 = 18) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_1\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOTPORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_12:\n" + "OR r25, %1\n" + // Wait 14 cycles (14 - 6 = 8) + + // 1 + 3 * N // + "LDI r26, 2\n" // 1 // set N + "_sleep_loop_12_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + "NOP\n" + + "OUT %0, r25\n" + // Wait 20 cycles (20 - 4 = 16) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_12_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" // 2 // + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 20 cycles (20 - 2 = 18) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_12_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_12\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23"); +#endif + break; + case DShot::Mode::DSHOT150: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + + "_for_loop_2:\n" + "OR r25, %1\n" + // Wait 28 cucles (28 - 6 = 22) + + // 1 + 3 * N // + "LDI r26, 7\n" // 1 // set N + "_sleep_loop_2_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "OUT %0, r25\n" + // Wait 40 cycles (40 - 4 = 36) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 40 cycles (40 - 2 = 38) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + "NOP\n" + "NOP\n" + + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_2\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23", "r26"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + + "_for_loop_22:\n" + "OR r25, %1\n" + // Wait 28 cucles (28 - 6 = 22) + + // 1 + 3 * N // + "LDI r26, 7\n" // 1 // set N + "_sleep_loop_22_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "OUT %0, r25\n" + // Wait 40 cycles (40 - 4 = 36) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_22_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 40 cycles (40 - 2 = 38) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_22_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + "NOP\n" + "NOP\n" + + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_22\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23", "r26"); +#endif + + break; + } + interrupts(); +} + +static boolean timerActive = false; + +/* + Generated by: + http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html + 500 Hz Update rate +*/ +static void initISR() { + cli(); // stop interrupts + TCCR1A = 0; // set entire TCCR1A register to 0 + TCCR1B = 0; // same for TCCR1B + TCNT1 = 0; // initialize counter value to 0 + // set compare match register for 500 Hz increments + OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) + // turn on CTC mode + TCCR1B |= (1 << WGM12); + // Set CS12, CS11 and CS10 bits for 1 prescaler + TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); + // enable timer compare interrupt + TIMSK1 |= (1 << OCIE1A); + timerActive = true; + for (uint8_t p = 0; p < 2; p++) { + for (uint8_t t = 0; t < MAX_TELEMETRY_REQUESTS; t++) { + for (uint8_t i = 0; i < 16; i++) { + // dShotBits[p][t][i] = 0; + } + } + dShotPins[p] = 0; + } + + sei(); // allow interrupts +} + +static boolean isTimerActive() { return timerActive; } + +ISR(TIMER1_COMPA_vect) { sendData(); } + +/* + Prepare data packet, attach 0 to telemetry bit, and calculate CRC + throttle: 11-bit data +*/ +static inline uint16_t createPacket(uint16_t throttle, bool telemetry) { + uint8_t csum = 0; + throttle <<= 1; + throttle |= telemetry; + uint16_t csum_data = throttle; + for (byte i = 0; i < 3; i++) { + csum ^= csum_data; + csum_data >>= 4; + } + csum &= 0xf; + return (throttle << 4) | csum; +} + +/****************** end of static functions *******************/ + +DShot::DShot(const enum Mode mode) { dShotMode = mode; } + +void DShot::attach(uint8_t pin) { + uint8_t pinMask = digitalPinToBitMask(pin); + uint8_t p = (digitalPinToPort(pin) == PORT1 ? 0 : 1); + pinMode(pin, OUTPUT); + if (!isTimerActive()) { + initISR(); + } + dShotPins[p] |= pinMask; +} + +/* + Set the throttle value and prepare the data packet and store + pin: pin of motor + throttle: 11-bit data +*/ +uint16_t DShot::setThrottle(uint8_t pin, uint16_t throttle, bool set_telemetry_bit) { + uint8_t bitMaskPin = digitalPinToBitMask(pin); + uint8_t p = (digitalPinToPort(pin) == PORT1 ? 0 : 1); + uint8_t dShotBitsTemp[16] = {0}; + memcpy(dShotBitsTemp, dShotBits[p], sizeof(dShotBitsTemp)); + + uint16_t packet = createPacket(throttle, 0); + uint16_t mask = 0x8000; + for (byte i = 0; i < 16; i++) { + if (packet & mask) + dShotBitsTemp[i] |= bitMaskPin; + else + dShotBitsTemp[i] &= ~bitMaskPin; + mask >>= 1; + } + memcpy(dShotBits[p], dShotBitsTemp, sizeof(dShotBitsTemp)); + + // do not request telemetry if there are already all slots reserved + if (set_telemetry_bit && dShotBitsT[p].getCount() < MAX_TELEMETRY_REQUESTS) { + dShotBitsTemp[11] |= bitMaskPin; // telemetry request bit + dShotBitsTemp[15] ^= bitMaskPin; // crc checksum + ++telemetryRequests[p]; + ++tlm_req_count; + + noInterrupts(); // to block unwanted states + telemetryRequestsPort.push(&p); + dShotBitsT[p].push(&dShotBitsTemp); + interrupts(); + } + +#ifdef DEBUG + DEBUG DEFINED ! // here to give compile error + uint8_t dShotBitsSend[2][16] = {0}; + if (telemetryRequestWait == 0) { + if (telemetryRequestsPort.getCount() > 0) { + uint8_t port; + telemetryRequestsPort.pop(&port); + p = port; + uint8_t dShotBitsTemp[16] = {0}; + if (port == 0) { + dShotBitsT[0].pop(&dShotBitsTemp); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + --telemetryRequests[0]; + memcpy(dShotBitsSend[0], dShotBitsTemp, sizeof(dShotBitsTemp)); + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + dShotBitsT[1].pop(&dShotBitsTemp); + memcpy(dShotBitsSend[1], dShotBitsTemp, sizeof(dShotBitsTemp)); + --telemetryRequests[1]; + } + telemetryRequestWait = TELEMETRY_WAIT; + --tlm_req_count; + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + } else { + telemetryRequestWait--; + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + return dShotBitsSend[p][11] + 200; +#endif + return 0; + // for debugging + return (dShotBitsT[0].getCount() * 10 + dShotBitsT[1].getCount()) * 100 + + telemetryRequestsPort.getCount(); +} + +/* + Send the ESC command immediatly, send motor speed 0 (command 48) afterwards + pin: pin of motor + command: 0-48, see Dshot_Digital_Cmd_Spec.txt +*/ +uint16_t DShot::sendCommand(uint8_t pin, uint16_t command) { + uint16_t sleepTime = 0; + noInterrupts(); + switch (command) { + case 1 ... 2: + sleepTime = 260; + setThrottle(pin, command, 0); + sendData(); + break; + case 3 ... 4: + sleepTime = 280; + setThrottle(pin, command, 0); + sendData(); + break; + case 5: + sleepTime = 1020; + setThrottle(pin, command, 0); + sendData(); + break; + case 6: + sleepTime = 12; + setThrottle(pin, command, 0); + sendData(); + break; + case 12: + sleepTime = 35; + case 7 ... 10: + case 13 ... 21: + case 32 ... 35: + setThrottle(pin, command, 0); + sendData(); // not tested yet + sendData(); + sendData(); + sendData(); + sendData(); + sendData(); + break; + default: // sends also not implemented commands once + setThrottle(pin, command, 0); + } + delay(sleepTime); + setThrottle(pin, 48, 0); // send motor speed 0 command + interrupts(); +} diff --git a/examples/DShotLibraryTest_8ESCs/DShot.h b/examples/DShotLibraryTest_8ESCs/DShot.h new file mode 100644 index 0000000..2f7f924 --- /dev/null +++ b/examples/DShotLibraryTest_8ESCs/DShot.h @@ -0,0 +1,52 @@ +#include "Arduino.h" +#include // Arduino Library from https://github.com/SMFSW/Queue +#include "queue.cpp" + +#ifndef DShot_h +#define DShot_h + +// #define DEBUG + +#if defined(__AVR_ATmega328P__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT1 PORTD +#define PORT1 digitalPinToPort(0) // Arduino Port number for PORTD +#endif + +#if defined(__AVR_ATmega8__) +// For UNO, PortD 0-7: i.e. D0-D7 +#define DSHOT_PORT1 PORTD +#define PORT1 digitalPinToPort(0) // Arduino Port number for PORTD +// ADDON for timers +#define TIMSK1 TIMSK +#endif + +#if defined(__AVR_ATmega32U4__) +// For Leonardo, PortB 4-7: i.e. D8-D11 +#define DSHOT_PORT1 PORTB +#define PORT1 digitalPinToPort(8) // Arduino Port number for PORTB +// More pins: PortF 18-21: i.e. D18-D21 +#define DSHOT_PORT2 PORTF +#define PORT2 digitalPinToPort(18) // Arduino Port number for PORTF +#endif + +class DShot { + public: + enum Mode { DSHOT600 = 4, DSHOT300 = 2, DSHOT150 = 1 }; + DShot(const enum Mode mode); + + // Initialises the output pin. Call once before sending anything + void attach(uint8_t pin); + + // Send throttle value. If set_telemetry_bit is true, it will request telemetry once, and + // afterwards sends the throttle value without telemetry request. + uint16_t setThrottle(uint8_t pin, uint16_t throttle, bool set_telemetry_bit); + + // Send commands from List as many times as needed, waiting the specified time before sending 48 + // command (0 speed) + uint16_t sendCommand(uint8_t pin, uint16_t command); + + private: +}; + +#endif diff --git a/examples/DShotLibraryTest_8ESCs/DShotKeywords.h b/examples/DShotLibraryTest_8ESCs/DShotKeywords.h new file mode 100644 index 0000000..80ada3d --- /dev/null +++ b/examples/DShotLibraryTest_8ESCs/DShotKeywords.h @@ -0,0 +1,115 @@ +/************************************************************************/ +/* DShot Commands */ +/* */ +/* Source: https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ */ +/* */ +/************************************************************************/ + +// Commands below are only executed when motors are stopped +#define DIGITAL_CMD_MOTOR_STOP 0 // Currently not implemented +#define DIGITAL_CMD_BEEP1 1 // Wait at least length of beep (260ms) before next command +#define DIGITAL_CMD_BEEP2 2 // Wait at least length of beep (260ms) before next command +#define DIGITAL_CMD_BEEP3 3 // Wait at least length of beep (280ms) before next command +#define DIGITAL_CMD_BEEP4 4 // Wait at least length of beep (280ms) before next command +#define DIGITAL_CMD_BEEP5 5 // Wait at least length of beep (1020ms) before next command +#define DIGITAL_CMD_ESC_INFO 6 // Wait at least 12ms before next command +#define DIGITAL_CMD_SPIN_DIRECTION_1 7 // Need 6x, no wait required +#define DIGITAL_CMD_SPIN_DIRECTION_2 8 // Need 6x, no wait required +#define DIGITAL_CMD_3D_MODE_OFF 9 // Need 6x, no wait required +#define DIGITAL_CMD_3D_MODE_ON 0 // Need 6x, no wait required +#define DIGITAL_CMD_SETTINGS_REQUEST 11 // Currently not implemented +#define DIGITAL_CMD_SAVE_SETTINGS 12 // Need 6x, wait at least 35ms before next command +#define DIGITAL_CMD_SPIN_DIRECTION_NORMAL 20 // Need 6x, no wait required +#define DIGITAL_CMD_SPIN_DIRECTION_REVERSED 21 // Need 6x, no wait required +#define DIGITAL_CMD_LED0_ON 22 // No wait required +#define DIGITAL_CMD_LED1_ON 23 // No wait required +#define DIGITAL_CMD_LED2_ON 24 // No wait required +#define DIGITAL_CMD_LED3_ON 25 // No wait required +#define DIGITAL_CMD_LED0_OFF 26 // No wait required +#define DIGITAL_CMD_LED1_OFF 27 // No wait required +#define DIGITAL_CMD_LED2_OFF 28 // No wait required +#define DIGITAL_CMD_LED3_OFF 29 // No wait required +#define AUDIO_STREAM_MODE 30 // Currently not implemented (Toggle on/off) +#define SILENT_MODE 31 // Currently not implemented (Toggle on/off) +#define DIGITAL_CMD_SIGNAL_LINE_TELEMETRY_DISABLE 32 // Need 6x, no wait required. Disables commands 42 to 47 +#define DIGITAL_CMD_SIGNAL_LINE_TELEMETRY_ENABLE 33 // Need 6x, no wait required. Enables commands 42 to 47 +#define DIGITAL_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY 34 \ + // Need 6x, no wait required. Enables commands 42 to 47, and sends erpm if normal Dshot frame +#define DIGITAL_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_PERIOD_TELEMETRY 35 \ + // Need 6x, no wait required. Enables commands 42 to 47, and sends erpm period if normal Dshot frame +// Commands above are only executed when motors are stopped + +/* +36 // Not yet assigned +37 // Not yet assigned +38 // Not yet assigned +39 // Not yet assigned +40 // Not yet assigned +41 // Not yet assigned +*/ + +// Commands below are executed at any time +#define DIGITAL_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY 42 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY 43 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_CURRENT_TELEMETRY 44 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY 45 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_ERPM_TELEMETRY 46 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY 47 // No wait required +// The above commands are valid for Dshot and Proshot input signals + +/* +DIGITAL_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY: 1°C per LSB +DIGITAL_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY: 10mV per LSB, 40.95V max +DIGITAL_CMD_SIGNAL_LINE_CURRENT_TELEMETRY: 100mA per LSB, 409.5A max +DIGITAL_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY: 10mAh per LSB, 40.95Ah max +DIGITAL_CMD_SIGNAL_LINE_ERPM_TELEMETRY: 100erpm per LSB, 409500erpm max +DIGITAL_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY: 16us per LSB, 65520us max TBD + + +ESC_INFO layout for BLHeli_32: +1-12: ESC SN +13: Indicates which response version is used. 254 is for BLHeli_32 version. +14: FW revision (32 = 32) +15: FW sub revision (10 = xx.1, 11 = xx.11) +16: Unused +17: Rotation direction reversed by dshot command or not (1:0) +18: 3D mode active or not (1:0) +19: Low voltage protection limit [0.1V] (255 = not capable, 0 = disabled) +20: Current protection limit [A] (255 = not capable, 0 = disabled) +21: LED0 on or not (1:0, 255 = LED0 not present) +22: LED1 on or not (1:0, 255 = LED1 not present) +23: LED2 on or not (1:0, 255 = LED2 not present) +24: LED3 on or not (1:0, 255 = LED3 not present) +25-31: Unused +32-63: ESC signature +64: CRC (same CRC as is used for telemetry) + +*/ + +/* +ESC Telemetry + Byte 0: Temperature + Byte 1: Voltage high byte + Byte 2: Voltage low byte + Byte 3: Current high byte + Byte 4: Current low byte + Byte 5: Consumption high byte + Byte 6: Consumption low byte + Byte 7: Rpm high byte + Byte 8: Rpm low byte + Byte 9: 8-bit CRC + +Converting the received values to standard units + int8_t Temperature = Temperature in 1 degree C + uint16_t Voltage = Volt *100 so 1000 are 10.00V + uint16_t Current = Ampere * 100 so 1000 are 10.00A + uint16_t Consumption = Consumption in 1mAh + uint16_t ERpm = Electrical Rpm /100 so 100 are 10000 Erpm + + note: to get the real Rpm of the motor you will need to divide the Erpm result + by the magnetpole count divided by two. + + So with a 14magnetpole motor: + Rpm = Erpm/7 + rpm = erpm / (motor poles/2) +*/ diff --git a/examples/DShotLibraryTest_8ESCs/DShotLibraryTest_8ESCs.ino b/examples/DShotLibraryTest_8ESCs/DShotLibraryTest_8ESCs.ino new file mode 100644 index 0000000..07ef96a --- /dev/null +++ b/examples/DShotLibraryTest_8ESCs/DShotLibraryTest_8ESCs.ino @@ -0,0 +1,283 @@ +#include "DShot.h" +/* + redefine DSHOT_PORT if you want to change the default PORT + + Defaults + UNO: PORTD, available pins 0-7 (D0-D7) + Leonardo: PORTB, available pins 4-7 (D8-D11) + PORTF, available pins A3-A0 + + e.g. + #define DSHOT_PORT PORTD +*/ + +// #define DEBUG 1 // Telemetry +// #define DEBUG2 1 +// #define DEBUG3 1 // Current debug +// #define DEBUG4 1 + +// Port B +#define M1 8 +#define M2 9 +#define M3 10 +#define M4 11 +// Port F +#define M5 18 +#define M6 19 +#define M7 20 +#define M8 21 + +#define NUM_ESC 8 +#define LEN_PAYLOAD (2 + NUM_ESC * 2) +#define TIMEOUT 15000 // ms (default was 100 if connected to program) +#define MAGNET_POLES 14 // Poles per Motor (needed for telemetry rpm calculations) + +// #define ANALOG_CURRENT // To add the currents in the telemetry message + +// Default values for DShot outputs +uint16_t const minDShot = 48; // without commands: 48, with commands: 1 (0 not implemented) +uint16_t const disarmCommand = 48; // without commands: 48, with commands: 1 (0 not implemented) +uint16_t const maxDShot = 2047; + +#ifdef ANALOG_CURRENT +int analogPin1 = A4; // current measurements from ESCs +int analogPin2 = A5; +#endif + +byte receivedIx = 0; +uint16_t receivedBytes[LEN_PAYLOAD]; +bool dataValid = false; + +bool armed = false; +bool throttleValid; +unsigned long lastValidMessage = 0; + +uint16_t target = 0; +uint16_t throttle = 0; + +DShot esc(DShot::Mode::DSHOT600); + +void setup() { + Serial.begin(115200); // communicating over usb + + Serial1.begin(115200); // Communication with ESC Telemetry + +#ifdef ANALOG_CURRENT + pinMode(analogPin1, INPUT); + pinMode(analogPin2, INPUT); + analogReference(INTERNAL); // INTERNAL: 2.5V (DEFAULT = 5V) +#endif + + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, HIGH); // LED On until first serial message received + + while (!Serial.available()) + ; + + while (!Serial1.available()) + Serial1.read(); + + // Notice, all pins must be connected to same PORT if possible + esc.attach(M1); + esc.setThrottle(M1, disarmCommand, 0); + esc.attach(M2); + esc.setThrottle(M2, disarmCommand, 0); + esc.attach(M3); + esc.setThrottle(M3, disarmCommand, 0); + esc.attach(M4); + esc.setThrottle(M4, disarmCommand, 0); + esc.attach(M5); + esc.setThrottle(M5, disarmCommand, 0); + esc.attach(M6); + esc.setThrottle(M6, disarmCommand, 0); + esc.attach(M7); + esc.setThrottle(M7, disarmCommand, 0); + esc.attach(M8); + esc.setThrottle(M8, disarmCommand, 0); + + // for visual reference + digitalWrite(LED_BUILTIN, LOW); + digitalWrite(LED_BUILTIN, HIGH); + delay(1000); + digitalWrite(LED_BUILTIN, LOW); + delay(200); + digitalWrite(LED_BUILTIN, HIGH); + delay(500); + digitalWrite(LED_BUILTIN, LOW); + /* + // to check if the ports are correct + if(digitalPinToPort(M5) == PORT2){ + digitalWrite(LED_BUILTIN, HIGH); + } + */ +} + +void setAllThrottles(uint16_t throttle, bool telemetry) { + esc.setThrottle(M1, throttle, telemetry); + esc.setThrottle(M2, throttle, telemetry); + esc.setThrottle(M3, throttle, telemetry); + esc.setThrottle(M4, throttle, telemetry); + esc.setThrottle(M5, throttle, telemetry); + esc.setThrottle(M6, throttle, telemetry); + esc.setThrottle(M7, throttle, telemetry); + esc.setThrottle(M8, throttle, telemetry); +} + +void loop() { + if (Serial.available() > 0) { + target = Serial.parseInt(); + + if (target < 0) { + target = 48; + } + + if (target > 2047) // safety measure, disarm when wrong input + target = 0; + Serial.print(target, DEC); + Serial.print("\t"); + Serial.print(throttle, DEC); + Serial.print("\n"); + } + + if (throttle < minDShot) { // special commands disabled + throttle = 48; + } + if (target <= 48) { + setAllThrottles(target, 0); + if (target == 0) + throttle = 48; + } else { + if (target > throttle) { + throttle++; + setAllThrottles(throttle, 0); + } else if (target < throttle) { + throttle--; + setAllThrottles(throttle, 0); + } + } + + readtelemetry(); + + delay(10); +} + +const inline uint8_t crc8(uint8_t *buffer, uint8_t BufLen) { + uint8_t remainder = 0; + const uint8_t polynomial = 0x07; + uint8_t byte, bit; + + for (byte = 0; byte < BufLen; ++byte) { + remainder ^= buffer[byte]; + for (bit = 0; bit < 8; ++bit) { + if (remainder & 0x80) { + remainder = (remainder << 1) ^ polynomial; + } else { + remainder = (remainder << 1); + } + } + } + return remainder; // returns zero if crc check is passed +} + +void readtelemetry() { + unsigned char serial_bridge = 0; + // send decoded telemetry back to computer (may not work properly!) + + uint8_t buffer[50]; + uint8_t n = 0; + + if (Serial1.available() >= 10) { + for (; Serial1.available() > 0 && serial_bridge < 50; serial_bridge++) { + buffer[serial_bridge] = Serial1.read(); + ++n; + } + + uint8_t startindex = 0; + + while (n > 10 && crc8(buffer[startindex], 10)) { + ++startindex; + --n; + } + if (!crc8(buffer[startindex], 10)) { // if it is a correct telemetry frame: + +#ifndef ANALOG_CURRENT + Serial.print("Temperature: "); + Serial.print(buffer[startindex]); + Serial.println(" °C"); + Serial.print("Voltage: "); + Serial.print((buffer[startindex + 1] << 8 | buffer[startindex + 2]) / 100); + Serial.println(" V"); + Serial.print("Current: "); + Serial.println((buffer[startindex + 3] << 8 | buffer[startindex + 4]) / 100); + Serial.println(" A"); + Serial.print("Consumption: "); + Serial.print(buffer[startindex + 5] << 8 | buffer[startindex + 6]); + Serial.println(" mAh"); + Serial.print("Velocity: "); + Serial.print((buffer[startindex + 7] << 8 | buffer[startindex + 8]) * 200 / MAGNET_POLES); + Serial.println(" rpm"); + Serial.println(""); + +#else + // Total current from both ESCs: + // Current : 15.2mv/A (datasheet ESC) + // analogRead: range 0-1023 for 0V to 2.53V (measured) + // Current in A = analogread * (2530 mV) /1023 / (15.2 mV/A) (= analogread + // * 0.16270515) + double current = + 0.16270515 * (analogRead(analogPin1) + analogRead(analogPin2)); + + Serial.print("Temperature: "); + Serial.print(buffer[startindex]); + Serial.println(" °C"); + Serial.print("Voltage: "); + Serial.print((buffer[startindex + 1] << 8 | buffer[startindex + 2]) / 100); + Serial.println(" V"); + Serial.print("Current: "); + Serial.print(current); + Serial.println(" A"); + Serial.print("Consumption: "); + Serial.print(buffer[startindex + 5] << 8 | buffer[startindex + 6]); + Serial.println(" mAh"); + Serial.print("Velocity: "); + Serial.print((buffer[startindex + 7] << 8 | buffer[startindex + 8]) * 200 / MAGNET_POLES); + Serial.println(" rpm"); + Serial.println(""); + +#endif // ANALOG_CURRENT + } + + // empty buffer for next reading + while (Serial1.available()) { + Serial1.read(); + } + } +} + +/* Telemetry info from https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ +ESC Telemetry + Byte 0: Temperature + Byte 1: Voltage high byte + Byte 2: Voltage low byte + Byte 3: Current high byte + Byte 4: Current low byte + Byte 5: Consumption high byte + Byte 6: Consumption low byte + Byte 7: Rpm high byte + Byte 8: Rpm low byte + Byte 9: 8-bit CRC + +Converting the received values to standard units + int8_t Temperature = Temperature in 1 degree C + uint16_t Voltage = Volt *100 so 1000 are 10.00V + uint16_t Current = Ampere * 100 so 1000 are 10.00A + uint16_t Consumption = Consumption in 1mAh + uint16_t ERpm = Electrical Rpm /100 so 100 are 10000 Erpm + + note: to get the real Rpm of the motor you will need to divide the Erpm result + by the magnetpole count divided by two. + + So with a 14magnetpole motor: + Rpm = Erpm/7 + rpm = erpm / (motor poles/2) +*/ diff --git a/examples/DShotLibraryTest_8ESCs/queue.cpp b/examples/DShotLibraryTest_8ESCs/queue.cpp new file mode 100644 index 0000000..5ef6759 --- /dev/null +++ b/examples/DShotLibraryTest_8ESCs/queue.cpp @@ -0,0 +1,10 @@ + +#include + +#define IMPLEMENTATION FIFO +#define MAX_TELEMETRY_REQUESTS 8 + +class Queue : public cppQueue { + public: + Queue() : cppQueue(sizeof(uint8_t[16]), MAX_TELEMETRY_REQUESTS, IMPLEMENTATION){}; +}; diff --git a/examples/telemetry_example/DShot.ino b/examples/telemetry_example/DShot.ino new file mode 100644 index 0000000..0aedcf2 --- /dev/null +++ b/examples/telemetry_example/DShot.ino @@ -0,0 +1,430 @@ +#include "DShot.h" +/* + THIS IS JUST AN EXAMPLE HOW THE CODE WAS USED + + redefine DSHOT_PORT if you want to change the default PORT + + Defaults + UNO: PORTD, available pins 0-7 (D0-D7) + Leonardo: PORTB, available pins 4-7 (D8-D11) + PORTF, available pins A3-A0 + + e.g. + #define DSHOT_PORT PORTD +*/ + +// #define DEBUG 1 // Telemetry +// #define DEBUG2 1 +// #define DEBUG3 1 // Current debug +// #define DEBUG4 1 + +// Port B +#define M1 8 +#define M2 9 +#define M3 10 +#define M4 11 +// Port F +#define M5 18 +#define M6 19 +#define M7 20 +#define M8 21 + +#define NUM_ESC 8 +#define LEN_PAYLOAD (2 + NUM_ESC * 2) +#define TIMEOUT 15000 // ms (default was 100 if connected to program) + +// #define ANALOG_CURRENT // To add the currents in the telemetry message + +// Default values for DShot outputs +uint16_t const disarmCommand[] = {48, 48, 48, 48, 48, 48, 48, 48}; +uint16_t const ARMCommand[] = {48, 48, 48, 48, 48, 48, 48, 48}; +uint16_t const minDShot = 48; // without commands: 48, with commands: 1 (0 not implemented) +uint16_t const maxDShot = 2047; +uint16_t currThrottle[NUM_ESC]; +bool currTelemetry[NUM_ESC]; +bool const noTelemetry[] = {0, 0, 0, 0, 0, 0, 0, 0}; + +#ifdef ANALOG_CURRENT +int analogPin1 = A4; // current measurements from ESCs +int analogPin2 = A5; +#endif + +byte receivedIx = 0; +byte receivedBytes[LEN_PAYLOAD]; +bool dataValid = false; + +bool armed = false; +bool throttleValid; +unsigned long lastValidMessage = 0; + +DShot esc(DShot::Mode::DSHOT600); + +void setup() { + Serial.begin(115200); // communicating over usb + + Serial1.begin(115200); // Communication with ESC Telemetry + +#ifdef ANALOG_CURRENT + pinMode(analogPin1, INPUT); + pinMode(analogPin2, INPUT); + analogReference(INTERNAL); // INTERNAL: 2.5V (DEFAULT = 5V) +#endif + + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, HIGH); // LED On until first serial message received + + while (!Serial.available()) + ; + + // Notice, all pins must be connected to same PORT if possible + esc.attach(M1); + esc.setThrottle(M1, disarmCommand[0], 0); + esc.attach(M2); + esc.setThrottle(M2, disarmCommand[0], 0); + esc.attach(M3); + esc.setThrottle(M3, disarmCommand[0], 0); + esc.attach(M4); + esc.setThrottle(M4, disarmCommand[0], 0); + esc.attach(M5); + esc.setThrottle(M5, disarmCommand[0], 0); + esc.attach(M6); + esc.setThrottle(M6, disarmCommand[0], 0); + esc.attach(M7); + esc.setThrottle(M7, disarmCommand[0], 0); + esc.attach(M8); + esc.setThrottle(M8, disarmCommand[0], 0); + + // for visual reference + digitalWrite(LED_BUILTIN, LOW); + digitalWrite(LED_BUILTIN, HIGH); + delay(1000); + digitalWrite(LED_BUILTIN, LOW); + delay(200); + digitalWrite(LED_BUILTIN, HIGH); + delay(500); + digitalWrite(LED_BUILTIN, LOW); + /* + // to check if the ports are correct + if(digitalPinToPort(M5) == PORT2){ + digitalWrite(LED_BUILTIN, HIGH); + } + */ +} + +void receiveData() { + // parse serial code - super simple protocol 0x26 followed by NUM_ESC*2 bytes and checksum (xor of + // all bytes) Magic pattern NUM_ESC* [0xAA 0xAA] arms the system every value between minDShot and + // maxDShot (interpreted as unsigned short -> 0x3E8 until 0x7D0) is a setpoint. MSB is telemetry + // request bit per ESC channel (e.g. 0x80 0x30 is telemetry request and motor off) + + while (Serial.available()) { + receivedBytes[receivedIx] = Serial.read(); + + if (receivedBytes[0] != 0x26) { + // nonsense received. Do nothing. + receivedIx = 0x00; + } + // last byte + else if (receivedIx == (LEN_PAYLOAD - 1)) { + // parse and check XOR of payload + byte checksum = receivedBytes[1]; + for (int i = 2; i < (LEN_PAYLOAD - 1); i++) { + checksum ^= receivedBytes[i]; + } + if (receivedBytes[LEN_PAYLOAD - 1] == checksum) { + lastValidMessage = millis(); + dataValid = true; +#ifdef DEBUG4 + // for (int i = 0; i < LEN_PAYLOAD - 1; ++i) { // last byte is just checksum + // Serial.print(receivedBytes[receivedIx + 1 - LEN_PAYLOAD + i], HEX); + // Serial.print(" "); + // } + Serial.print("dataValid"); +#endif + } + // reset ptr + receivedIx = 0; + return; + } else { + receivedIx++; + } + } +} + +const inline bool timeOutReached() { return (millis() - TIMEOUT > lastValidMessage); } + +bool dataIsArmingSequence() { + // check each payload byte + for (int i = 1; i < LEN_PAYLOAD - 1; i++) { + // if any byte is not the magic value, we don't + // have an arming sequence. + if (receivedBytes[i] != 0xAA) { + return false; + } + } + return true; +} + +bool dataIsDisarmingSequence() { + // check each payload byte + for (int i = 1; i < LEN_PAYLOAD - 1; i++) { + // if any byte is not the magic value, we don't + // have an arming sequence. + if (receivedBytes[i] != 0xFF) { + return false; + } + } + return true; +} + +bool updateCurrThrottleFromData() { + uint16_t currThrottleTemp[NUM_ESC]; + bool telemetryTemp[NUM_ESC]; + + for (int i = 0; i < NUM_ESC; i++) { + int payloadIx = i * 2 + 1; + currThrottleTemp[i] = + (((uint16_t)(receivedBytes[payloadIx + 1] & 0x7F)) << 8) | receivedBytes[payloadIx]; + + telemetryTemp[i] = receivedBytes[payloadIx + 1] >> 7; + + // check if invalid + if (currThrottleTemp[i] < minDShot || currThrottleTemp[i] > maxDShot) { + return false; + } + } + + // if we made it to here, all throttles are valid! + memcpy(currThrottle, currThrottleTemp, sizeof(currThrottle[0]) * NUM_ESC); + memcpy(currTelemetry, telemetryTemp, sizeof(currTelemetry[0]) * NUM_ESC); + return true; +} + +void finishData() { + if (dataValid) { + receivedIx = 0; + dataValid = false; + } +} + +void arm() { + armed = true; + memcpy(currThrottle, ARMCommand, sizeof(currThrottle[0]) * NUM_ESC); + digitalWrite(LED_BUILTIN, HIGH); // LED on when armed +} + +void disarm() { + armed = false; + memcpy(currThrottle, disarmCommand, sizeof(currThrottle[0]) * NUM_ESC); + digitalWrite(LED_BUILTIN, LOW); // LED off when disarmed +} + +void updateESC() { +#ifndef DEBUG + + // test setup + esc.setThrottle(M1, (armed ? currThrottle[4] : disarmCommand[0]), currTelemetry[4]); + esc.setThrottle(M2, (armed ? currThrottle[5] : disarmCommand[0]), currTelemetry[5]); + esc.setThrottle(M3, (armed ? currThrottle[6] : disarmCommand[0]), currTelemetry[6]); + esc.setThrottle(M4, (armed ? currThrottle[7] : disarmCommand[0]), currTelemetry[7]); + + esc.setThrottle(M8, (armed ? currThrottle[0] : disarmCommand[0]), currTelemetry[0]); + esc.setThrottle(M7, (armed ? currThrottle[1] : disarmCommand[0]), currTelemetry[1]); + esc.setThrottle(M6, (armed ? currThrottle[2] : disarmCommand[0]), currTelemetry[2]); + esc.setThrottle(M5, (armed ? currThrottle[3] : disarmCommand[0]), currTelemetry[3]); + +#else + if(currTelemetry[0] == 1){ Serial.print("########## NEW TELEMETRY :) ###########"); } + Serial.print("telemetry requests: "); + Serial.print("0:"); + Serial.print(esc.setThrottle(M5, (armed ? currThrottle[0] : disarmCommand[0]), currTelemetry[0]), DEC); + Serial.print(" 1:"); + Serial.print(esc.setThrottle(M3, (armed ? currThrottle[1] : disarmCommand[0]), currTelemetry[1]), DEC); + Serial.print(" 2:"); + Serial.print(esc.setThrottle(M2, (armed ? currThrottle[2] : disarmCommand[0]), currTelemetry[2]), DEC); + Serial.print(" 3:"); + Serial.print(esc.setThrottle(M7, (armed ? currThrottle[3] : disarmCommand[0]), currTelemetry[3]), DEC); + Serial.print(" 4:"); + Serial.print(esc.setThrottle(M6, (armed ? currThrottle[4] : disarmCommand[0]), currTelemetry[4]), DEC); + Serial.print(" 5:"); + Serial.print(esc.setThrottle(M4, (armed ? currThrottle[5] : disarmCommand[0]), currTelemetry[5]), DEC); + Serial.print(" 6:"); + Serial.print(esc.setThrottle(M1, (armed ? currThrottle[6] : disarmCommand[0]), currTelemetry[6]), DEC); + Serial.print(" 7:"); + Serial.println(esc.setThrottle(M8, (armed ? currThrottle[7] : disarmCommand[0]), currTelemetry[7]), DEC); +// for(int i = 0; i < 8; ++i){ +// if(currTelemetry[i] == 1){ +// Serial.print("telemetry request on:"); +// Serial.print(i, DEC); +// // Serial.print("]: "); +// // Serial.println(tr, DEC); +// } +// } +#endif // DEBUG +} + +const inline uint8_t crc8(uint8_t* buffer, uint8_t BufLen) { + uint8_t remainder = 0; + const uint8_t polynomial = 0x07; + uint8_t byte, bit; + + for (byte = 0; byte < BufLen; ++byte) { + remainder ^= buffer[byte]; + for (bit = 0; bit < 8; ++bit) { + if (remainder & 0x80) { + remainder = (remainder << 1) ^ polynomial; + } else { + remainder = (remainder << 1); + } + } + } + return remainder; // returns zero if crc check is passed +} + +void loop() { + receiveData(); + + // new data package and we're armed. + if (armed && dataValid) { + if (dataIsDisarmingSequence()) { + disarm(); + } else { + // try to update current throttle. + throttleValid = updateCurrThrottleFromData(); + } + if (!throttleValid) { + disarm(); + +#ifdef DEBUG2 // if something was invalid, we print it out on the serial. + Serial.print("Invalid data: "); + Serial.print(receivedBytes[1], HEX); + for (uint8_t i = 1; i < LEN_PAYLOAD - 1; i += 2) { + Serial.print(receivedBytes[i], HEX); + Serial.print(receivedBytes[i + 1], HEX); + Serial.print(", "); + } + Serial.println(receivedBytes[LEN_PAYLOAD - 1], HEX); +#endif + } + } + // new data package while we're not armed + else if (!armed && dataValid) { + // check if we should arm + if (dataIsArmingSequence()) { + arm(); + } + } else // no new data + { + // check if we reached timeout + if (armed && timeOutReached()) { + disarm(); + } + } + + // update current state to ESCs + updateESC(); + memcpy(currTelemetry, noTelemetry, sizeof(noTelemetry[0]) * NUM_ESC); // reset telemetry request + + // finish data if it was valid + finishData(); + + + unsigned char serial_bridge = 0; +// send telemetry back to computer +#ifndef ANALOG_CURRENT // the telemetry frame can just be forwarded as it is + + for (; Serial1.available() > 0 && serial_bridge < 10; serial_bridge++) { + Serial.write(Serial1.read()); + } + +#else // add current measurements in telemetry frame (does not work properly yet) + + uint8_t buffer[50]; + uint8_t n = 0; + + if (Serial1.available() >= 10) { + for (; Serial1.available() > 0 && serial_bridge < 50; serial_bridge++) { + buffer[serial_bridge] = Serial1.read(); + ++n; + +#ifdef DEBUG3 + Serial.print(buffer[serial_bridge], HEX); + Serial.print(" "); +#endif + } + + uint8_t startindex = 0; + + while (n > 10 && crc8(buffer[startindex], 10)) { +#ifdef DEBUG3 + Serial.print("CRC="); + Serial.print(crc8(buffer[startindex], 10), HEX); +#endif + ++startindex; + --n; + } + if (!crc8(buffer[startindex], 10)) { // if it is a correct telemetry frame: + + // Total current from both ESCs: + // Current : 15.2mv/A + // analogRead: range 0-1023 for 0V to 2.53V (measured) + // Current in A = analogread * (2530 mV) /1023 / (15.2 mV/A) (= analogread * 0.16270515) + double current = 0.16270515 * (analogRead(analogPin1) + analogRead(analogPin2)); + + // send current in correct frame (our used ESCs send current only analog) + buffer[3 + startindex] = (uint16_t)(current * 100.) >> 8; + buffer[4 + startindex] = (uint16_t)(current * 100.) & 0xFF; + + // send changed CRC8 + buffer[9 + startindex] = crc8(buffer[startindex], 9); + +#ifdef DEBUG3 + Serial.println("Buffer: "); + for (int i = 0; i < 10; ++i) { + Serial.print(buffer[i + startindex], HEX); + Serial.print(" "); + } + Serial.println("end."); +#else + + for (uint8_t i = 0; i < 10; i++) { + Serial.write(buffer[i + startindex]); + } +#endif + } + + // empty buffer for next reading + while (Serial1.available()) { + Serial1.read(); + } + } +#endif // ANALOG_CURRENT +} + +/* Telemetry info from https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ +ESC Telemetry + Byte 0: Temperature + Byte 1: Voltage high byte + Byte 2: Voltage low byte + Byte 3: Current high byte + Byte 4: Current low byte + Byte 5: Consumption high byte + Byte 6: Consumption low byte + Byte 7: Rpm high byte + Byte 8: Rpm low byte + Byte 9: 8-bit CRC + +Converting the received values to standard units + int8_t Temperature = Temperature in 1 degree C + uint16_t Voltage = Volt *100 so 1000 are 10.00V + uint16_t Current = Ampere * 100 so 1000 are 10.00A + uint16_t Consumption = Consumption in 1mAh + uint16_t ERpm = Electrical Rpm /100 so 100 are 10000 Erpm + + note: to get the real Rpm of the motor you will need to divide the Erpm result + by the magnetpole count divided by two. + + So with a 14magnetpole motor: + Rpm = Erpm/7 + rpm = erpm / (motor poles/2) +*/ diff --git a/src/DShot.cpp b/src/DShot.cpp index 12e9a49..ad12446 100644 --- a/src/DShot.cpp +++ b/src/DShot.cpp @@ -1,293 +1,621 @@ -#include "Arduino.h" #include "DShot.h" +// TODO: See if timing works for sequence of telemetry requests: DSHOT600: t_period = x us, +// t_telemetry_packet = x ms? Telemetry sent back over one UART --> one: first port has priority +#define MAX_TELEMETRY_REQUESTS 8 +#define TELEMETRY_WAIT 0 // 4* dShotMode + // TELEMETRY_SEND_TIME/DSHOT150_PERIOD * (dShotMode+1) --> since the dshot + // commands are sent at 500 Hz rate, no wait is required. +//------------------------------------------------ +#define DSHOT150_PERIOD 247 // us +#define TELEMETRY_SEND_TIME 900 // us + +// Denote which pins are attached to dShot +static uint8_t dShotPins[] = {0, 0}; // Each item contains the bit of the port // Pins that are not attached will always be 1 // This is to offload the Off pattern calculation during bit send -static uint8_t dShotBits[16]; +static uint8_t dShotBits[2][16]; +// counter, how many telemetry requests have to be done. first come first served. +static uint8_t telemetryRequests[] = {0, 0}; + +static uint8_t tlm_req_count = 0; + +static uint8_t dShotBitsTemp[16] = {0}; -// Denote which pin is attached to dShot -static uint8_t dShotPins = 0; +static Queue dShotBitsT[2]; // FIFO Queue for telemetry bits +static cppQueue telemetryRequestsPort(sizeof(uint8_t), 2 * MAX_TELEMETRY_REQUESTS, + FIFO); // FIFO Queue for telemetry port + +// counter to set to the wait time in DSHOTperiod units +static uint16_t telemetryRequestWait = 0; // Mode: 600/300/150 static enum DShot::Mode dShotMode = DShot::Mode::DSHOT600; - -#define NOP "NOP\n" +#define NOP "NOP\n" #define NOP2 NOP NOP #define NOP4 NOP2 NOP2 #define NOP8 NOP4 NOP4 /* - DSHOT600 implementation - For 16MHz CPU, - 0: 10 cycle ON, 17 cycle OFF - 1: 20 cycle ON, 7 cycle OFF - Total 27 cycle each bit + DSHOT600 implementation + For 16MHz CPU, + 0: 10 cycle ON, 17 cycle OFF + 1: 20 cycle ON, 7 cycle OFF + Total 27 cycle each bit */ -static inline void sendData(){ - noInterrupts(); - switch (dShotMode) { - case DShot::Mode::DSHOT600: - default: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - "_for_loop_0:\n" - "OR r25, %1\n" - // Wait 7 cycles (7 - 6 = 1) - "NOP\n" - - "OUT %0, r25\n" - // Wait 10 cycles (10 - 4 = 6) - NOP4 - NOP2 - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 10 cycles (10 - 2 = 8) - NOP8 - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_0\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23" - ); - break; - case DShot::Mode::DSHOT300: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - "_for_loop_1:\n" - "OR r25, %1\n" - // Wait 14 cycles (14 - 6 = 8) - - // 1 + 3 * N // - "LDI r26, 2\n" // 1 // set N - "_sleep_loop_1_1:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_1\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - "NOP\n" - - "OUT %0, r25\n" - // Wait 20 cycles (20 - 4 = 16) - - // 1 + 3 * N // - "LDI r26, 5\n" // 1 // set N - "_sleep_loop_1_2:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_2\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" // 2 // - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 20 cycles (20 - 2 = 18) - - // 1 + 3 * N // - "LDI r26, 5\n" // 1 // set N - "_sleep_loop_1_3:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_1_3\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_1\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23" - ); - break; - case DShot::Mode::DSHOT150: - asm( - // For i = 0 to 15: - "LDI r23, 0\n" - // Set High for every attached pins - // DSHOT_PORT |= dShotPins; - "IN r25, %0\n" - - "_for_loop_2:\n" - "OR r25, %1\n" - // Wait 28 cucles (28 - 6 = 22) - - // 1 + 3 * N // - "LDI r26, 7\n" // 1 // set N - "_sleep_loop_2_1:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_1\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "OUT %0, r25\n" - // Wait 40 cycles (40 - 4 = 36) - - // 1 + 3 * N // - "LDI r26, 11\n" // 1 // set N - "_sleep_loop_2_2:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_2\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - - // Set Low for low bits only - //DSHOT_PORT &= dShotBits[i]; - "LD r24, Z+\n" - "AND r25, r24\n" - "OUT %0, r25\n" - // Wait 40 cycles (40 - 2 = 38) - - // 1 + 3 * N // - "LDI r26, 11\n" // 1 // set N - "_sleep_loop_2_3:\n" - "DEC r26\n" // 1 // - "BRNE _sleep_loop_2_3\n" // 2 // - "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 - // 1 + 3 * N // - - "NOP\n" - "NOP\n" - "NOP\n" - "NOP\n" - - // Turn off everything - // DSHOT_PORT &= ~dShotPins; - "AND r25, %2\n" - "OUT %0, r25\n" - // Add to i (tmp_reg) - "INC r23\n" - "CPI r23, 16\n" - "BRLO _for_loop_2\n" - // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no wait - : - : "I" (_SFR_IO_ADDR(DSHOT_PORT)), "r" (dShotPins), "r" (~dShotPins), "z" (dShotBits) - : "r25", "r24", "r23", "r26" - ); - break; - } - interrupts(); +static inline void sendData() { + noInterrupts(); + + // Logic for sending one telemetry request at a time + uint8_t dShotBitsSend[2][16] = {0}; +#ifndef DEBUG + if (telemetryRequestWait == 0) { + if (telemetryRequestsPort.getCount() > 0) { + uint8_t port; + telemetryRequestsPort.pop(&port); + if (port == 0) { + // if (telemetryRequests[0] > 0) { + dShotBitsT[0].pop(dShotBitsSend[0]); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + --telemetryRequests[0]; + // } else if (telemetryRequests[1] > 0) { + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + dShotBitsT[1].pop(dShotBitsSend[1]); + --telemetryRequests[1]; + } + telemetryRequestWait = TELEMETRY_WAIT; // for debug + --tlm_req_count; + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + } else { + telemetryRequestWait--; + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } +#endif + + switch (dShotMode) { + case DShot::Mode::DSHOT600: + default: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_0:\n" + "OR r25, %1\n" + // Wait 7 cycles (7 - 6 = 1) + "NOP\n" + + "OUT %0, r25\n" + // Wait 10 cycles (10 - 4 = 6) + NOP4 NOP2 + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 10 cycles (10 - 2 = 8) + NOP8 + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_0\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_02:\n" + "OR r25, %1\n" + // Wait 7 cycles (7 - 6 = 1) + "NOP\n" + + "OUT %0, r25\n" + // Wait 10 cycles (10 - 4 = 6) + NOP4 NOP2 + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 10 cycles (10 - 2 = 8) + NOP8 + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_02\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23"); +#endif + break; + case DShot::Mode::DSHOT300: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOTPORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_1:\n" + "OR r25, %1\n" + // Wait 14 cycles (14 - 6 = 8) + + // 1 + 3 * N // + "LDI r26, 2\n" // 1 // set N + "_sleep_loop_1_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + "NOP\n" + + "OUT %0, r25\n" + // Wait 20 cycles (20 - 4 = 16) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" // 2 // + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 20 cycles (20 - 2 = 18) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_1_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_1_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_1\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOTPORT |= dShotPins; + "IN r25, %0\n" + "_for_loop_12:\n" + "OR r25, %1\n" + // Wait 14 cycles (14 - 6 = 8) + + // 1 + 3 * N // + "LDI r26, 2\n" // 1 // set N + "_sleep_loop_12_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + "NOP\n" + + "OUT %0, r25\n" + // Wait 20 cycles (20 - 4 = 16) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_12_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" // 2 // + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 20 cycles (20 - 2 = 18) + + // 1 + 3 * N // + "LDI r26, 5\n" // 1 // set N + "_sleep_loop_12_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_12_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_12\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23"); +#endif + break; + case DShot::Mode::DSHOT150: + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + + "_for_loop_2:\n" + "OR r25, %1\n" + // Wait 28 cucles (28 - 6 = 22) + + // 1 + 3 * N // + "LDI r26, 7\n" // 1 // set N + "_sleep_loop_2_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "OUT %0, r25\n" + // Wait 40 cycles (40 - 4 = 36) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 40 cycles (40 - 2 = 38) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_2_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_2_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + "NOP\n" + "NOP\n" + + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_2\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT1)), "r"(dShotPins[0]), "r"(~dShotPins[0]), + "z"(dShotBitsSend[0]) + : "r25", "r24", "r23", "r26"); + +#ifdef DSHOT_PORT2 + asm( + // For i = 0 to 15: + "LDI r23, 0\n" + // Set High for every attached pins + // DSHOT_PORT |= dShotPins; + "IN r25, %0\n" + + "_for_loop_22:\n" + "OR r25, %1\n" + // Wait 28 cucles (28 - 6 = 22) + + // 1 + 3 * N // + "LDI r26, 7\n" // 1 // set N + "_sleep_loop_22_1:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_1\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "OUT %0, r25\n" + // Wait 40 cycles (40 - 4 = 36) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_22_2:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_2\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + + // Set Low for low bits only + // DSHOT_PORT &= dShotBits[i]; + "LD r24, Z+\n" + "AND r25, r24\n" + "OUT %0, r25\n" + // Wait 40 cycles (40 - 2 = 38) + + // 1 + 3 * N // + "LDI r26, 11\n" // 1 // set N + "_sleep_loop_22_3:\n" + "DEC r26\n" // 1 // + "BRNE _sleep_loop_22_3\n" // 2 // + "NOP\n" // 1 // BRNE on skip uses only 1 cycle, not 2 + // 1 + 3 * N // + + "NOP\n" + "NOP\n" + "NOP\n" + "NOP\n" + + // Turn off everything + // DSHOT_PORT &= ~dShotPins; + "AND r25, %2\n" + "OUT %0, r25\n" + // Add to i (tmp_reg) + "INC r23\n" + "CPI r23, 16\n" + "BRLO _for_loop_22\n" + // 7 cycles to next bit (4 to add to i and branch, 2 to turn on), no + // wait + : + : "I"(_SFR_IO_ADDR(DSHOT_PORT2)), "r"(dShotPins[1]), "r"(~dShotPins[1]), + "z"(dShotBitsSend[1]) + : "r25", "r24", "r23", "r26"); +#endif + + break; + } + interrupts(); } static boolean timerActive = false; + /* Generated by: http://www.8bit-era.cz/arduino-timer-interrupts-calculator.html - 1000 Hz Update rate + 500 Hz Update rate */ -static void initISR(){ -cli(); // stop interrupts -TCCR1A = 0; // set entire TCCR1A register to 0 -TCCR1B = 0; // same for TCCR1B -TCNT1 = 0; // initialize counter value to 0 -// set compare match register for 500 Hz increments -OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) -// turn on CTC mode -TCCR1B |= (1 << WGM12); -// Set CS12, CS11 and CS10 bits for 1 prescaler -TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); -// enable timer compare interrupt -TIMSK1 |= (1 << OCIE1A); - timerActive = true; - for (byte i=0; i<16; i++){ - dShotBits[i] = 0; - } - dShotPins = 0; - - sei(); // allow interrupts -} +static void initISR() { + cli(); // stop interrupts + TCCR1A = 0; // set entire TCCR1A register to 0 + TCCR1B = 0; // same for TCCR1B + TCNT1 = 0; // initialize counter value to 0 + // set compare match register for 500 Hz increments + OCR1A = 31999; // = 16000000 / (1 * 500) - 1 (must be <65536) + // turn on CTC mode + TCCR1B |= (1 << WGM12); + // Set CS12, CS11 and CS10 bits for 1 prescaler + TCCR1B |= (0 << CS12) | (0 << CS11) | (1 << CS10); + // enable timer compare interrupt + TIMSK1 |= (1 << OCIE1A); + timerActive = true; + for (uint8_t p = 0; p < 2; p++) { + for (uint8_t t = 0; t < MAX_TELEMETRY_REQUESTS; t++) { + for (uint8_t i = 0; i < 16; i++) { + // dShotBits[p][t][i] = 0; + } + } + dShotPins[p] = 0; + } -static boolean isTimerActive(){ - return timerActive; + sei(); // allow interrupts } -ISR(TIMER1_COMPA_vect){ - sendData(); -} +static boolean isTimerActive() { return timerActive; } + +ISR(TIMER1_COMPA_vect) { sendData(); } /* Prepare data packet, attach 0 to telemetry bit, and calculate CRC throttle: 11-bit data */ -static inline uint16_t createPacket(uint16_t throttle){ +static inline uint16_t createPacket(uint16_t throttle, bool telemetry) { uint8_t csum = 0; throttle <<= 1; - // Indicate as command if less than 48 - if (throttle < 48 && throttle > 0) - throttle |= 1; + throttle |= telemetry; uint16_t csum_data = throttle; - for (byte i=0; i<3; i++){ + for (byte i = 0; i < 3; i++) { csum ^= csum_data; csum_data >>= 4; } csum &= 0xf; - return (throttle<<4)|csum; + return (throttle << 4) | csum; } /****************** end of static functions *******************/ -DShot::DShot(const enum Mode mode){ - dShotMode = mode; -} +DShot::DShot(const enum Mode mode) { dShotMode = mode; } -void DShot::attach(uint8_t pin){ - this->_packet = 0; - this->_pinMask = digitalPinToBitMask(pin); +void DShot::attach(uint8_t pin) { + uint8_t pinMask = digitalPinToBitMask(pin); + uint8_t p = (digitalPinToPort(pin) == PORT1 ? 0 : 1); pinMode(pin, OUTPUT); - if (!isTimerActive()){ + if (!isTimerActive()) { initISR(); } - dShotPins |= this->_pinMask; + dShotPins[p] |= pinMask; } /* Set the throttle value and prepare the data packet and store + pin: pin of motor throttle: 11-bit data */ -uint16_t DShot::setThrottle(uint16_t throttle){ - this->_throttle = throttle; +uint16_t DShot::setThrottle(uint8_t pin, uint16_t throttle, bool set_telemetry_bit) { + uint8_t bitMaskPin = digitalPinToBitMask(pin); + uint8_t p = (digitalPinToPort(pin) == PORT1 ? 0 : 1); + uint8_t dShotBitsTemp[16] = {0}; + memcpy(dShotBitsTemp, dShotBits[p], sizeof(dShotBitsTemp)); - // TODO: This part can be further optimized when combine with create packet - this->_packet = createPacket(throttle); + uint16_t packet = createPacket(throttle, 0); uint16_t mask = 0x8000; - for (byte i=0; i<16; i++){ - if (this->_packet & mask) - dShotBits[i] |= this->_pinMask; + for (byte i = 0; i < 16; i++) { + if (packet & mask) + dShotBitsTemp[i] |= bitMaskPin; else - dShotBits[i] &= ~(this->_pinMask); + dShotBitsTemp[i] &= ~bitMaskPin; mask >>= 1; } - return _packet; + memcpy(dShotBits[p], dShotBitsTemp, sizeof(dShotBitsTemp)); + + // do not request telemetry if there are already all slots reserved + if (set_telemetry_bit && dShotBitsT[p].getCount() < MAX_TELEMETRY_REQUESTS) { + dShotBitsTemp[11] |= bitMaskPin; // telemetry request bit + dShotBitsTemp[15] ^= bitMaskPin; // crc checksum + ++telemetryRequests[p]; + ++tlm_req_count; + + noInterrupts(); // to block unwanted states + telemetryRequestsPort.push(&p); + dShotBitsT[p].push(&dShotBitsTemp); + interrupts(); + } + +#ifdef DEBUG + DEBUG DEFINED ! // here to give compile error + uint8_t dShotBitsSend[2][16] = {0}; + if (telemetryRequestWait == 0) { + if (telemetryRequestsPort.getCount() > 0) { + uint8_t port; + telemetryRequestsPort.pop(&port); + p = port; + uint8_t dShotBitsTemp[16] = {0}; + if (port == 0) { + dShotBitsT[0].pop(&dShotBitsTemp); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + --telemetryRequests[0]; + memcpy(dShotBitsSend[0], dShotBitsTemp, sizeof(dShotBitsTemp)); + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + dShotBitsT[1].pop(&dShotBitsTemp); + memcpy(dShotBitsSend[1], dShotBitsTemp, sizeof(dShotBitsTemp)); + --telemetryRequests[1]; + } + telemetryRequestWait = TELEMETRY_WAIT; + --tlm_req_count; + } else { + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + } else { + telemetryRequestWait--; + memcpy(dShotBitsSend[0], dShotBits[0], sizeof(dShotBits[0])); + memcpy(dShotBitsSend[1], dShotBits[1], sizeof(dShotBits[1])); + } + return dShotBitsSend[p][11] + 200; +#endif + return 0; + // for debugging + return (dShotBitsT[0].getCount() * 10 + dShotBitsT[1].getCount()) * 100 + + telemetryRequestsPort.getCount(); +} + +/* + Send the ESC command immediatly, send motor speed 0 (command 48) afterwards + pin: pin of motor + command: 0-48, see Dshot_Digital_Cmd_Spec.txt +*/ +uint16_t DShot::sendCommand(uint8_t pin, uint16_t command) { + uint16_t sleepTime = 0; + noInterrupts(); + switch (command) { + case 1 ... 2: + sleepTime = 260; + setThrottle(pin, command, 0); + sendData(); + break; + case 3 ... 4: + sleepTime = 280; + setThrottle(pin, command, 0); + sendData(); + break; + case 5: + sleepTime = 1020; + setThrottle(pin, command, 0); + sendData(); + break; + case 6: + sleepTime = 12; + setThrottle(pin, command, 0); + sendData(); + break; + case 12: + sleepTime = 35; + case 7 ... 10: + case 13 ... 21: + case 32 ... 35: + setThrottle(pin, command, 0); + sendData(); // not tested yet + sendData(); + sendData(); + sendData(); + sendData(); + sendData(); + break; + default: // sends also not implemented commands once + setThrottle(pin, command, 0); + } + delay(sleepTime); + setThrottle(pin, 48, 0); // send motor speed 0 command + interrupts(); } diff --git a/src/DShot.h b/src/DShot.h index d3e81a6..2f7f924 100644 --- a/src/DShot.h +++ b/src/DShot.h @@ -1,39 +1,52 @@ #include "Arduino.h" +#include // Arduino Library from https://github.com/SMFSW/Queue +#include "queue.cpp" #ifndef DShot_h #define DShot_h +// #define DEBUG + #if defined(__AVR_ATmega328P__) // For UNO, PortD 0-7: i.e. D0-D7 -#define DSHOT_PORT PORTD +#define DSHOT_PORT1 PORTD +#define PORT1 digitalPinToPort(0) // Arduino Port number for PORTD #endif #if defined(__AVR_ATmega8__) // For UNO, PortD 0-7: i.e. D0-D7 -#define DSHOT_PORT PORTD +#define DSHOT_PORT1 PORTD +#define PORT1 digitalPinToPort(0) // Arduino Port number for PORTD // ADDON for timers #define TIMSK1 TIMSK #endif #if defined(__AVR_ATmega32U4__) // For Leonardo, PortB 4-7: i.e. D8-D11 -#define DSHOT_PORT PORTB +#define DSHOT_PORT1 PORTB +#define PORT1 digitalPinToPort(8) // Arduino Port number for PORTB +// More pins: PortF 18-21: i.e. D18-D21 +#define DSHOT_PORT2 PORTF +#define PORT2 digitalPinToPort(18) // Arduino Port number for PORTF #endif -class DShot{ - public: - enum Mode { - DSHOT600, - DSHOT300, - DSHOT150 - }; - DShot(const enum Mode mode); - void attach(uint8_t pin); - uint16_t setThrottle(uint16_t throttle); - private: - uint16_t _packet = 0; - uint16_t _throttle = 0; - uint8_t _pinMask = 0; +class DShot { + public: + enum Mode { DSHOT600 = 4, DSHOT300 = 2, DSHOT150 = 1 }; + DShot(const enum Mode mode); + + // Initialises the output pin. Call once before sending anything + void attach(uint8_t pin); + + // Send throttle value. If set_telemetry_bit is true, it will request telemetry once, and + // afterwards sends the throttle value without telemetry request. + uint16_t setThrottle(uint8_t pin, uint16_t throttle, bool set_telemetry_bit); + + // Send commands from List as many times as needed, waiting the specified time before sending 48 + // command (0 speed) + uint16_t sendCommand(uint8_t pin, uint16_t command); + + private: }; #endif diff --git a/src/DShotKeywords.h b/src/DShotKeywords.h new file mode 100644 index 0000000..80ada3d --- /dev/null +++ b/src/DShotKeywords.h @@ -0,0 +1,115 @@ +/************************************************************************/ +/* DShot Commands */ +/* */ +/* Source: https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ */ +/* */ +/************************************************************************/ + +// Commands below are only executed when motors are stopped +#define DIGITAL_CMD_MOTOR_STOP 0 // Currently not implemented +#define DIGITAL_CMD_BEEP1 1 // Wait at least length of beep (260ms) before next command +#define DIGITAL_CMD_BEEP2 2 // Wait at least length of beep (260ms) before next command +#define DIGITAL_CMD_BEEP3 3 // Wait at least length of beep (280ms) before next command +#define DIGITAL_CMD_BEEP4 4 // Wait at least length of beep (280ms) before next command +#define DIGITAL_CMD_BEEP5 5 // Wait at least length of beep (1020ms) before next command +#define DIGITAL_CMD_ESC_INFO 6 // Wait at least 12ms before next command +#define DIGITAL_CMD_SPIN_DIRECTION_1 7 // Need 6x, no wait required +#define DIGITAL_CMD_SPIN_DIRECTION_2 8 // Need 6x, no wait required +#define DIGITAL_CMD_3D_MODE_OFF 9 // Need 6x, no wait required +#define DIGITAL_CMD_3D_MODE_ON 0 // Need 6x, no wait required +#define DIGITAL_CMD_SETTINGS_REQUEST 11 // Currently not implemented +#define DIGITAL_CMD_SAVE_SETTINGS 12 // Need 6x, wait at least 35ms before next command +#define DIGITAL_CMD_SPIN_DIRECTION_NORMAL 20 // Need 6x, no wait required +#define DIGITAL_CMD_SPIN_DIRECTION_REVERSED 21 // Need 6x, no wait required +#define DIGITAL_CMD_LED0_ON 22 // No wait required +#define DIGITAL_CMD_LED1_ON 23 // No wait required +#define DIGITAL_CMD_LED2_ON 24 // No wait required +#define DIGITAL_CMD_LED3_ON 25 // No wait required +#define DIGITAL_CMD_LED0_OFF 26 // No wait required +#define DIGITAL_CMD_LED1_OFF 27 // No wait required +#define DIGITAL_CMD_LED2_OFF 28 // No wait required +#define DIGITAL_CMD_LED3_OFF 29 // No wait required +#define AUDIO_STREAM_MODE 30 // Currently not implemented (Toggle on/off) +#define SILENT_MODE 31 // Currently not implemented (Toggle on/off) +#define DIGITAL_CMD_SIGNAL_LINE_TELEMETRY_DISABLE 32 // Need 6x, no wait required. Disables commands 42 to 47 +#define DIGITAL_CMD_SIGNAL_LINE_TELEMETRY_ENABLE 33 // Need 6x, no wait required. Enables commands 42 to 47 +#define DIGITAL_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY 34 \ + // Need 6x, no wait required. Enables commands 42 to 47, and sends erpm if normal Dshot frame +#define DIGITAL_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_PERIOD_TELEMETRY 35 \ + // Need 6x, no wait required. Enables commands 42 to 47, and sends erpm period if normal Dshot frame +// Commands above are only executed when motors are stopped + +/* +36 // Not yet assigned +37 // Not yet assigned +38 // Not yet assigned +39 // Not yet assigned +40 // Not yet assigned +41 // Not yet assigned +*/ + +// Commands below are executed at any time +#define DIGITAL_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY 42 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY 43 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_CURRENT_TELEMETRY 44 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY 45 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_ERPM_TELEMETRY 46 // No wait required +#define DIGITAL_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY 47 // No wait required +// The above commands are valid for Dshot and Proshot input signals + +/* +DIGITAL_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY: 1°C per LSB +DIGITAL_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY: 10mV per LSB, 40.95V max +DIGITAL_CMD_SIGNAL_LINE_CURRENT_TELEMETRY: 100mA per LSB, 409.5A max +DIGITAL_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY: 10mAh per LSB, 40.95Ah max +DIGITAL_CMD_SIGNAL_LINE_ERPM_TELEMETRY: 100erpm per LSB, 409500erpm max +DIGITAL_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY: 16us per LSB, 65520us max TBD + + +ESC_INFO layout for BLHeli_32: +1-12: ESC SN +13: Indicates which response version is used. 254 is for BLHeli_32 version. +14: FW revision (32 = 32) +15: FW sub revision (10 = xx.1, 11 = xx.11) +16: Unused +17: Rotation direction reversed by dshot command or not (1:0) +18: 3D mode active or not (1:0) +19: Low voltage protection limit [0.1V] (255 = not capable, 0 = disabled) +20: Current protection limit [A] (255 = not capable, 0 = disabled) +21: LED0 on or not (1:0, 255 = LED0 not present) +22: LED1 on or not (1:0, 255 = LED1 not present) +23: LED2 on or not (1:0, 255 = LED2 not present) +24: LED3 on or not (1:0, 255 = LED3 not present) +25-31: Unused +32-63: ESC signature +64: CRC (same CRC as is used for telemetry) + +*/ + +/* +ESC Telemetry + Byte 0: Temperature + Byte 1: Voltage high byte + Byte 2: Voltage low byte + Byte 3: Current high byte + Byte 4: Current low byte + Byte 5: Consumption high byte + Byte 6: Consumption low byte + Byte 7: Rpm high byte + Byte 8: Rpm low byte + Byte 9: 8-bit CRC + +Converting the received values to standard units + int8_t Temperature = Temperature in 1 degree C + uint16_t Voltage = Volt *100 so 1000 are 10.00V + uint16_t Current = Ampere * 100 so 1000 are 10.00A + uint16_t Consumption = Consumption in 1mAh + uint16_t ERpm = Electrical Rpm /100 so 100 are 10000 Erpm + + note: to get the real Rpm of the motor you will need to divide the Erpm result + by the magnetpole count divided by two. + + So with a 14magnetpole motor: + Rpm = Erpm/7 + rpm = erpm / (motor poles/2) +*/ diff --git a/src/Dshot_Digital_Cmd_Spec.txt b/src/Dshot_Digital_Cmd_Spec.txt deleted file mode 100644 index 6221a38..0000000 --- a/src/Dshot_Digital_Cmd_Spec.txt +++ /dev/null @@ -1,78 +0,0 @@ -/************************************************************************/ -/* DShot Commands */ -/* */ -/* Source: https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ */ -/* */ -/************************************************************************/ - -0 DIGITAL_CMD_MOTOR_STOP // Currently not implemented -1 DIGITAL_CMD_BEEP1 // Wait at least length of beep (260ms) before next command -2 DIGITAL_CMD_BEEP2 // Wait at least length of beep (260ms) before next command -3 DIGITAL_CMD_BEEP3 // Wait at least length of beep (280ms) before next command -4 DIGITAL_CMD_BEEP4 // Wait at least length of beep (280ms) before next command -5 DIGITAL_CMD_BEEP5 // Wait at least length of beep (1020ms) before next command -6 DIGITAL_CMD_ESC_INFO // Wait at least 12ms before next command -7 DIGITAL_CMD_SPIN_DIRECTION_1 // Need 6x, no wait required -8 DIGITAL_CMD_SPIN_DIRECTION_2 // Need 6x, no wait required -9 DIGITAL_CMD_3D_MODE_OFF // Need 6x, no wait required -10 DIGITAL_CMD_3D_MODE_ON // Need 6x, no wait required -11 DIGITAL_CMD_SETTINGS_REQUEST // Currently not implemented -12 DIGITAL_CMD_SAVE_SETTINGS // Need 6x, wait at least 35ms before next command -20 DIGITAL_CMD_SPIN_DIRECTION_NORMAL // Need 6x, no wait required -21 DIGITAL_CMD_SPIN_DIRECTION_REVERSED // Need 6x, no wait required -22 DIGITAL_CMD_LED0_ON // No wait required -23 DIGITAL_CMD_LED1_ON // No wait required -24 DIGITAL_CMD_LED2_ON // No wait required -25 DIGITAL_CMD_LED3_ON // No wait required -26 DIGITAL_CMD_LED0_OFF // No wait required -27 DIGITAL_CMD_LED1_OFF // No wait required -28 DIGITAL_CMD_LED2_OFF // No wait required -29 DIGITAL_CMD_LED3_OFF // No wait required -30 Audio_Stream mode on/Off // Currently not implemented -31 Silent Mode on/Off // Currently not implemented -32 DIGITAL_CMD_SIGNAL_LINE_TELEMETRY_DISABLE // Need 6x, no wait required. Disables commands 42 to 47 -33 DIGITAL_CMD_SIGNAL_LINE_TELEMETRY_ENABLE // Need 6x, no wait required. Enables commands 42 to 47 -34 DIGITAL_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_TELEMETRY // Need 6x, no wait required. Enables commands 42 to 47, and sends erpm if normal Dshot frame -35 DIGITAL_CMD_SIGNAL_LINE_CONTINUOUS_ERPM_PERIOD_TELEMETRY // Need 6x, no wait required. Enables commands 42 to 47, and sends erpm period if normal Dshot frame -Commands above are only executed when motors are stopped -36 // Not yet assigned -37 // Not yet assigned -38 // Not yet assigned -39 // Not yet assigned -40 // Not yet assigned -41 // Not yet assigned -Commands below are executed at any time -42 DIGITAL_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY // No wait required -43 DIGITAL_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY // No wait required -44 DIGITAL_CMD_SIGNAL_LINE_CURRENT_TELEMETRY // No wait required -45 DIGITAL_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY // No wait required -46 DIGITAL_CMD_SIGNAL_LINE_ERPM_TELEMETRY // No wait required -47 DIGITAL_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY // No wait required - -The above commands are valid for Dshot and Proshot input signals - -DIGITAL_CMD_SIGNAL_LINE_TEMPERATURE_TELEMETRY: 1°C per LSB -DIGITAL_CMD_SIGNAL_LINE_VOLTAGE_TELEMETRY: 10mV per LSB, 40.95V max -DIGITAL_CMD_SIGNAL_LINE_CURRENT_TELEMETRY: 100mA per LSB, 409.5A max -DIGITAL_CMD_SIGNAL_LINE_CONSUMPTION_TELEMETRY: 10mAh per LSB, 40.95Ah max -DIGITAL_CMD_SIGNAL_LINE_ERPM_TELEMETRY: 100erpm per LSB, 409500erpm max -DIGITAL_CMD_SIGNAL_LINE_ERPM_PERIOD_TELEMETRY: 16us per LSB, 65520us max TBD - - -ESC_INFO layout for BLHeli_32: -1-12: ESC SN -13: Indicates which response version is used. 254 is for BLHeli_32 version. -14: FW revision (32 = 32) -15: FW sub revision (10 = xx.1, 11 = xx.11) -16: Unused -17: Rotation direction reversed by dshot command or not (1:0) -18: 3D mode active or not (1:0) -19: Low voltage protection limit [0.1V] (255 = not capable, 0 = disabled) -20: Current protection limit [A] (255 = not capable, 0 = disabled) -21: LED0 on or not (1:0, 255 = LED0 not present) -22: LED1 on or not (1:0, 255 = LED1 not present) -23: LED2 on or not (1:0, 255 = LED2 not present) -24: LED3 on or not (1:0, 255 = LED3 not present) -25-31: Unused -32-63: ESC signature -64: CRC (same CRC as is used for telemetry) diff --git a/src/queue.cpp b/src/queue.cpp new file mode 100644 index 0000000..dcd49a4 --- /dev/null +++ b/src/queue.cpp @@ -0,0 +1,9 @@ +#include + +#define IMPLEMENTATION FIFO +#define MAX_TELEMETRY_REQUESTS 8 + +class Queue : public cppQueue { + public: + Queue() : cppQueue(sizeof(uint8_t[16]), MAX_TELEMETRY_REQUESTS, IMPLEMENTATION){}; +};