From ea559618fdd55d5b52fc193df76efacf88899737 Mon Sep 17 00:00:00 2001 From: Wastl Kraus Date: Tue, 29 Jun 2021 20:05:20 +0200 Subject: [PATCH] ...init --- DShot_Lib.sln | 15 +++ DShot_Lib.vcxitems | 28 +++++ DShot_Lib.vcxitems.filters | 30 ++++++ library.properties | 9 ++ readme.txt | 6 ++ src/BlheliCmdMap.h | 30 ++++++ src/DShot_Lib.cpp | 214 +++++++++++++++++++++++++++++++++++++ src/DShot_Lib.h | 101 +++++++++++++++++ 8 files changed, 433 insertions(+) create mode 100644 DShot_Lib.sln create mode 100644 DShot_Lib.vcxitems create mode 100644 DShot_Lib.vcxitems.filters create mode 100644 library.properties create mode 100644 readme.txt create mode 100644 src/BlheliCmdMap.h create mode 100644 src/DShot_Lib.cpp create mode 100644 src/DShot_Lib.h diff --git a/DShot_Lib.sln b/DShot_Lib.sln new file mode 100644 index 0000000..f241aee --- /dev/null +++ b/DShot_Lib.sln @@ -0,0 +1,15 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.31313.79 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DShot_Lib", "DShot_Lib.vcxitems", "{C8B1B533-6DCB-41EB-A603-F7E4448734BD}" +EndProject +Global + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {AC2A4B9B-18F8-4ED8-81F6-7D1970F2F3C9} + EndGlobalSection +EndGlobal diff --git a/DShot_Lib.vcxitems b/DShot_Lib.vcxitems new file mode 100644 index 0000000..362902d --- /dev/null +++ b/DShot_Lib.vcxitems @@ -0,0 +1,28 @@ + + + + $(MSBuildAllProjects);$(MSBuildThisFileFullPath) + true + {c8b1b533-6dcb-41eb-a603-f7e4448734bd} + + + + %(AdditionalIncludeDirectories);$(MSBuildThisFileDirectory) + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/DShot_Lib.vcxitems.filters b/DShot_Lib.vcxitems.filters new file mode 100644 index 0000000..6691e41 --- /dev/null +++ b/DShot_Lib.vcxitems.filters @@ -0,0 +1,30 @@ + + + + + {4FC737F1-C7A5-4376-A066-2A32D752A2FF} + cpp;c;cc;cxx;s + + + {93995380-89BD-4b04-88EB-625FBE52EBFB} + h;hh;hpp;hxx + + + + + Source Files + + + + + + + Header Files + + + + + Header Files + + + \ No newline at end of file diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..981bb1d --- /dev/null +++ b/library.properties @@ -0,0 +1,9 @@ +name=DShot_Lib +version=0.2.0 +author=derdoktor667 +maintainer=derdoktor667 +sentence=DShot_Lib Library supporting all DShot Types and speeds. Tested with BlHeli_S. +paragraph=This library can control a BlHeli_S by using encoded DShot commands like Betaflight. +category=Device Control +url=https://github/DShot_Lib +architectures=esp32 diff --git a/readme.txt b/readme.txt new file mode 100644 index 0000000..0d8e450 --- /dev/null +++ b/readme.txt @@ -0,0 +1,6 @@ +# README + +This library can control a BlHeli_S by using encoded DShot commands. +More to come + +--- diff --git a/src/BlheliCmdMap.h b/src/BlheliCmdMap.h new file mode 100644 index 0000000..7192e48 --- /dev/null +++ b/src/BlheliCmdMap.h @@ -0,0 +1,30 @@ +#pragma once + +// source: https://github.com/bitdump/BLHeli/blob/master/BLHeli_S%20SiLabs/Dshotprog%20spec%20BLHeli_S.txt + +enum dshot_cmd_t { + DIGITAL_CMD_MOTOR_STOP, // Currently not implemented + DIGITAL_CMD_BEEP1, // Wait at least length of beep (380ms) before next command + DIGITAL_CMD_BEEP2, // Wait at least length of beep (380ms) before next command + DIGITAL_CMD_BEEP3, // Wait at least length of beep (400ms) before next command + DIGITAL_CMD_BEEP4, // Wait at least length of beep (400ms) before next command + DIGITAL_CMD_BEEP5, // Wait at least length of beep (400ms) before next command + DIGITAL_CMD_ESC_INFO, // Wait at least 12ms before next command + DIGITAL_CMD_SPIN_DIRECTION_1, // Currently not implemented + DIGITAL_CMD_SPIN_DIRECTION_2, // Need 6x, no wait required + DIGITAL_CMD_3D_MODE_OFF, // Need 6x, no wait required + DIGITAL_CMD_3D_MODE_ON, // Need 6x, no wait required + DIGITAL_CMD_SETTINGS_REQUEST, // Currently not implemented + DIGITAL_CMD_SAVE_SETTINGS, // Need 6x, wait at least 12ms before next command + DIGITAL_CMD_SPIN_DIRECTION_NORMAL = 20, // Need 6x, no wait required + DIGITAL_CMD_SPIN_DIRECTION_REVERSED, // Need 6x, no wait required + DIGITAL_CMD_LED0_ON, // No wait required + DIGITAL_CMD_LED1_ON, // No wait required + DIGITAL_CMD_LED2_ON, // No wait required + DIGITAL_CMD_LED3_ON, // No wait required + DIGITAL_CMD_LED0_OFF, // No wait required + DIGITAL_CMD_LED1_OFF, // No wait required + DIGITAL_CMD_LED2_OFF, // No wait required + DIGITAL_CMD_LED3_OFF, // No wait required + DSHOT_CMD_MAX = 47 +}; diff --git a/src/DShot_Lib.cpp b/src/DShot_Lib.cpp new file mode 100644 index 0000000..50056cd --- /dev/null +++ b/src/DShot_Lib.cpp @@ -0,0 +1,214 @@ +/* + Name: DShot_Lib.cpp + Created: 29.06.2021 19:41:44 + Author: derdoktor667 + Editor: http://www.visualmicro.com +*/ + +#include "DShot_Lib.h" + +DShotRMT::DShotRMT(gpio_num_t gpio, rmt_channel_t rmtChannel) { + dshot_config.gpio_num = gpio; + dshot_config.pin_num = uint8_t(gpio); + dshot_config.rmt_channel = rmtChannel; + dshot_config.mem_block_num = uint8_t(RMT_CHANNEL_MAX - uint8_t(rmtChannel)); + + // ...create clean packet + encode_dshot_to_rmt(DSHOT_NULL_PACKET); +} + +DShotRMT::DShotRMT(uint8_t pin, uint8_t channel) { + dshot_config.gpio_num = gpio_num_t(pin); + dshot_config.pin_num = pin; + dshot_config.rmt_channel = rmt_channel_t(channel); + dshot_config.mem_block_num = (RMT_CHANNEL_MAX - channel); + + // ...create clean packet + encode_dshot_to_rmt(DSHOT_NULL_PACKET); +} + +DShotRMT::~DShotRMT() { + rmt_driver_uninstall(dshot_config.rmt_channel); +} + +DShotRMT::DShotRMT(DShotRMT const&) { + // ...write me +} + +DShotRMT& DShotRMT::operator=(DShotRMT const&) { + // TODO: hier return-Anweisung eingeben +} + +bool DShotRMT::begin(dshot_mode_t dshot_mode, bool is_bidirectional) { + dshot_config.mode = dshot_mode; + dshot_config.clk_div = DSHOT_CLK_DIVIDER; + dshot_config.name_str = dshot_mode_name[dshot_mode]; + dshot_config.is_inverted = is_bidirectional; + + switch (dshot_config.mode) { + case DSHOT150: + dshot_config.ticks_per_bit = 64; // ...Bit Period Time 6.67 µs + dshot_config.ticks_zero_high = 24; // ...zero time 2.50 µs + dshot_config.ticks_one_high = 48; // ...one time 5.00 µs + break; + + case DSHOT300: + dshot_config.ticks_per_bit = 32; // ...Bit Period Time 3.33 µs + dshot_config.ticks_zero_high = 12; // ...zero time 1.25 µs + dshot_config.ticks_one_high = 24; // ...one time 2.50 µs + break; + + case DSHOT600: + dshot_config.ticks_per_bit = 16; // ...Bit Period Time 1.67 µs + dshot_config.ticks_zero_high = 6; // ...zero time 0.625 µs + dshot_config.ticks_one_high = 12; // ...one time 1.25 µs + break; + + case DSHOT1200: + dshot_config.ticks_per_bit = 8; // ...Bit Period Time 0.83 µs + dshot_config.ticks_zero_high = 3; // ...zero time 0.313 µs + dshot_config.ticks_one_high = 6; // ...one time 0.625 µs + break; + + // ...because having a default is "good style" + default: + dshot_config.ticks_per_bit = 0; // ...Bit Period Time endless + dshot_config.ticks_zero_high = 0; // ...no bits, no time + dshot_config.ticks_one_high = 0; // ......no bits, no time + break; + } + + // ...calc low signal timing + dshot_config.ticks_zero_low = (dshot_config.ticks_per_bit - dshot_config.ticks_zero_high); + dshot_config.ticks_one_low = (dshot_config.ticks_per_bit - dshot_config.ticks_one_high); + + rmt_dshot_config.rmt_mode = RMT_MODE_TX; + rmt_dshot_config.channel = dshot_config.rmt_channel; + rmt_dshot_config.gpio_num = dshot_config.gpio_num; + rmt_dshot_config.mem_block_num = dshot_config.mem_block_num; + rmt_dshot_config.clk_div = dshot_config.clk_div; + + rmt_dshot_config.tx_config.loop_en = false; + rmt_dshot_config.tx_config.carrier_en = false; + rmt_dshot_config.tx_config.idle_level = RMT_IDLE_LEVEL_LOW; + rmt_dshot_config.tx_config.idle_output_en = true; + + // ...setup selected dshot mode + rmt_config(&rmt_dshot_config); + + // ...essential step, return the result + auto init_failed = rmt_driver_install(rmt_dshot_config.channel, 0, 0); + + // ...because esp_err_t returns more than true or false + if (init_failed != 0) { + return true; + } + else { + return false; + } +} + +void DShotRMT::send_dshot_value(uint16_t throttle_value, telemetric_request_t telemetric_request) { + dshot_packet_t dshot_rmt_packet = { }; + + if (throttle_value < DSHOT_THROTTLE_MIN) { + throttle_value = DSHOT_THROTTLE_MIN; + } + + if (throttle_value > DSHOT_THROTTLE_MAX) { + throttle_value = DSHOT_THROTTLE_MAX; + } + + if (dshot_config.is_inverted) { + + // ...implement bidirectional mode + + } + else { + dshot_rmt_packet.throttle_value = throttle_value; + dshot_rmt_packet.telemetric_request = telemetric_request; + dshot_rmt_packet.checksum = this->calc_dshot_chksum(dshot_rmt_packet); + + output_rmt_data(dshot_rmt_packet); + } +} + +dshot_config_t* DShotRMT::get_dshot_info() { + return &dshot_config; +} + +uint8_t DShotRMT::get_dshot_clock_div() { + return dshot_config.clk_div; +} + +rmt_item32_t* DShotRMT::encode_dshot_to_rmt(uint16_t parsed_packet) { + for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1) { + if (parsed_packet & 0b1000000000000000) { + // set one + dshot_rmt_item[i].duration0 = dshot_config.ticks_one_high; + dshot_rmt_item[i].level0 = 1; + dshot_rmt_item[i].duration1 = dshot_config.ticks_one_low; + dshot_rmt_item[i].level1 = 0; + } + else { + // set zero + dshot_rmt_item[i].duration0 = dshot_config.ticks_zero_high; + dshot_rmt_item[i].level0 = 1; + dshot_rmt_item[i].duration1 = dshot_config.ticks_zero_low; + dshot_rmt_item[i].level1 = 0; + } + } + + // ...end marker added to each frame + dshot_rmt_item[DSHOT_PAUSE_BIT].duration0 = DSHOT_PAUSE_BIDIRECTIONAL; + dshot_rmt_item[DSHOT_PAUSE_BIT].level0 = 0; + dshot_rmt_item[DSHOT_PAUSE_BIT].duration1 = 0; + dshot_rmt_item[DSHOT_PAUSE_BIT].level1 = 0; + + return dshot_rmt_item; +} + +// ...just returns the checksum +// DOES NOT APPEND CHECKSUM!!! +uint16_t DShotRMT::calc_dshot_chksum(const dshot_packet_t& dshot_packet) { + uint16_t packet = DSHOT_NULL_PACKET; + uint16_t chksum = DSHOT_NULL_PACKET; + + if (dshot_config.is_inverted) { + + // ...implement bidirectional mode + + } + else { + packet = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request; + + for (int i = 0; i < 3; i++) { + chksum ^= packet; // xor data by nibbles + packet >>= 4; + } + + chksum &= 0b0000000000001111; + } + + return chksum; +} + +uint16_t DShotRMT::prepare_rmt_data(const dshot_packet_t& dshot_packet) { + uint16_t prepared_to_encode = DSHOT_NULL_PACKET; + + uint16_t chksum = calc_dshot_chksum(dshot_packet); + + prepared_to_encode = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request; + prepared_to_encode = (prepared_to_encode << 4) | chksum; + + return prepared_to_encode; +} + +// ...finally output using ESP32 RMT +void DShotRMT::output_rmt_data(const dshot_packet_t& dshot_packet) { + encode_dshot_to_rmt(prepare_rmt_data(dshot_packet)); + + // + rmt_write_items(rmt_dshot_config.channel, dshot_rmt_item, DSHOT_PACKET_LENGTH, false); +} + diff --git a/src/DShot_Lib.h b/src/DShot_Lib.h new file mode 100644 index 0000000..5def015 --- /dev/null +++ b/src/DShot_Lib.h @@ -0,0 +1,101 @@ +/* + Name: DShot_Lib.h + Created: 29.06.2021 19:41:44 + Author: derdoktor667 +*/ + +#pragma once + +#include "BlheliCmdMap.h" +#include + +#include +#include + +constexpr auto DSHOT_CLK_DIVIDER = 8; // ...slow down RMT clock to 10 ticks => 1ns +constexpr auto DSHOT_PACKET_LENGTH = 17; // ...last pack is the pause + +constexpr auto DSHOT_THROTTLE_MIN = 48; +constexpr auto DSHOT_THROTTLE_MAX = 2047; +constexpr auto DSHOT_NULL_PACKET = 0b0000000000000000; + +constexpr auto DSHOT_PAUSE = (DSHOT_PACKET_LENGTH * 21); // ...21bit is recommended +constexpr auto DSHOT_PAUSE_BIDIRECTIONAL = DSHOT_PACKET_LENGTH; +constexpr auto DSHOT_PAUSE_BIT = 16; + +constexpr auto F_CPU_RMT = 80000000L; +constexpr auto RMT_CYCLES_PER_SEC = (F_CPU_RMT / DSHOT_CLK_DIVIDER); +constexpr auto RMT_CYCLES_PER_ESP_CYCLE = (F_CPU / RMT_CYCLES_PER_SEC); + +typedef enum dshot_mode_e { + DSHOT_OFF, + DSHOT150, + DSHOT300, + DSHOT600, + DSHOT1200 +} dshot_mode_t; + +static const char* const dshot_mode_name[] = { + "DSHOT_OFF", + "DSHOT150", + "DSHOT300", + "DSHOT600", + "DSHOT1200" +}; + +typedef enum request_e { + NO_TELEMETRIC, + ENABLE_TELEMETRIC, +} telemetric_request_t; + +typedef struct dshot_packet_s { + uint16_t throttle_value : 11; + telemetric_request_t telemetric_request : 1; + uint16_t checksum : 4; +} dshot_packet_t; + +typedef String dshot_name_t; + +typedef struct dshot_config_s { + dshot_mode_t mode; + dshot_name_t name_str; + bool is_inverted; + gpio_num_t gpio_num; + uint8_t pin_num; + rmt_channel_t rmt_channel; + uint8_t mem_block_num; + uint16_t ticks_per_bit; + uint8_t clk_div; + uint16_t ticks_zero_high; + uint16_t ticks_zero_low; + uint16_t ticks_one_high; + uint16_t ticks_one_low; +} dshot_config_t; + +class DShotRMT { + public: + DShotRMT(gpio_num_t gpio, rmt_channel_t rmtChannel); + DShotRMT(uint8_t pin, uint8_t channel); + ~DShotRMT(); + DShotRMT(DShotRMT const&); + DShotRMT& operator=(DShotRMT const&); + + bool begin(dshot_mode_t dshot_mode = DSHOT_OFF, bool is_bidirectional = false); + void send_dshot_value(uint16_t throttle_value, telemetric_request_t telemetric_request = NO_TELEMETRIC); + + dshot_config_t* get_dshot_info(); + uint8_t get_dshot_clock_div(); + + private: + rmt_item32_t dshot_rmt_item[DSHOT_PACKET_LENGTH] = { }; + dshot_config_t dshot_config = { }; + rmt_config_t rmt_dshot_config = { }; + + rmt_item32_t* encode_dshot_to_rmt(uint16_t parsed_packet); + uint16_t calc_dshot_chksum(const dshot_packet_t& dshot_packet); + uint16_t prepare_rmt_data(const dshot_packet_t& dshot_packet); + + void output_rmt_data(const dshot_packet_t& dshot_packet); +}; + +