src/lib/ServoOutput/DShotRMT.cpp

   1 #if (defined(GPIO_PIN_PWM_OUTPUTS) && defined(PLATFORM_ESP32))
   2 //
   3 // Name:                DShotRMT.cpp
   4 // Created:     20.03.2021 00:49:15
   5 // Author:      derdoktor667
   6 //
   7
   8 #include "DShotRMT.h"
   9
  10 DShotRMT::DShotRMT(gpio_num_t gpio, rmt_channel_t rmtChannel) : gpio_num(gpio), rmt_channel(rmtChannel) {
  11         // ...create clean packet
  12         encode_dshot_to_rmt(DSHOT_NULL_PACKET);
  13 }
  14
  15 DShotRMT::~DShotRMT() {
  16         rmt_tx_stop(rmt_channel);
  17         rmt_driver_uninstall(rmt_channel);
  18 }
  19
  20 bool DShotRMT::begin(dshot_mode_t dshot_mode, bool is_bidirectional) {
  21         mode = dshot_mode;
  22         bidirectional = is_bidirectional;
  23
  24         uint16_t ticks_per_bit;
  25
  26         switch (mode) {
  27                 case DSHOT150:
  28                         ticks_per_bit = 64; // ...Bit Period Time 6.67 us
  29                         ticks_zero_high = 24; // ...zero time 2.50 us
  30                         ticks_one_high = 48; // ...one time 5.00 us
  31                         break;
  32
  33                 case DSHOT300:
  34                         ticks_per_bit = 32; // ...Bit Period Time 3.33 us
  35                         ticks_zero_high = 12; // ...zero time 1.25 us
  36                         ticks_one_high = 24; // ...one time 2.50 us
  37                         break;
  38
  39                 case DSHOT600:
  40                         ticks_per_bit = 16; // ...Bit Period Time 1.67 us
  41                         ticks_zero_high = 6; // ...zero time 0.625 us
  42                         ticks_one_high = 12; // ...one time 1.25 us
  43                         break;
  44
  45                 case DSHOT1200:
  46                         ticks_per_bit = 8; // ...Bit Period Time 0.83 us
  47                         ticks_zero_high = 3; // ...zero time 0.313 us
  48                         ticks_one_high = 6; // ...one time 0.625 us
  49                         break;
  50
  51                 // ...because having a default is "good style"
  52                 default:
  53                         ticks_per_bit = 0; // ...Bit Period Time endless
  54                         ticks_zero_high = 0; // ...no bits, no time
  55                         ticks_one_high = 0; // ......no bits, no time
  56                         break;
  57         }
  58
  59         // ...calc low signal timing
  60         ticks_zero_low = (ticks_per_bit - ticks_zero_high);
  61         ticks_one_low = (ticks_per_bit - ticks_one_high);
  62
  63         rmt_config_t dshot_tx_rmt_config = {
  64                 .rmt_mode = RMT_MODE_TX,
  65                 .channel = rmt_channel,
  66                 .gpio_num = gpio_num,
  67                 .clk_div = DSHOT_CLK_DIVIDER,
  68                 .mem_block_num = uint8_t(RMT_CHANNEL_MAX - uint8_t(rmt_channel)),
  69                 .tx_config = {
  70                 .idle_level = bidirectional ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW,
  71                         .carrier_en = false,
  72                         .loop_en = true,
  73                         .idle_output_en = true,
  74                 },
  75         };
  76
  77         // ...pause "bit" added to each frame
  78     if (bidirectional) {
  79         dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level0 = HIGH;
  80         dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level1 = HIGH;
  81     } else {
  82         dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level0 = LOW;
  83         dshot_tx_rmt_item[DSHOT_PAUSE_BIT].level1 = LOW;
  84     }
  85
  86         dshot_tx_rmt_item[DSHOT_PAUSE_BIT].duration1 = 0;
  87         dshot_tx_rmt_item[DSHOT_PAUSE_BIT].duration0 = 10000 - (16*ticks_per_bit) - 1;
  88
  89         // setup the RMT end marker
  90         dshot_tx_rmt_item[DSHOT_PACKET_LENGTH-1].duration0 = 0;
  91         dshot_tx_rmt_item[DSHOT_PACKET_LENGTH-1].level0 = HIGH;
  92         dshot_tx_rmt_item[DSHOT_PACKET_LENGTH-1].duration1 = 0;
  93         dshot_tx_rmt_item[DSHOT_PACKET_LENGTH-1].level1 = LOW;
  94
  95         // ...setup selected dshot mode
  96         rmt_config(&dshot_tx_rmt_config);
  97
  98         // ...essential step, return the result
  99         return rmt_driver_install(dshot_tx_rmt_config.channel, 0, 0);
 100 }
 101
 102 // ...the config part is done, now the calculating and sending part
 103 void DShotRMT::send_dshot_value(uint16_t throttle_value, telemetric_request_t telemetric_request) {
 104         dshot_packet_t dshot_rmt_packet = { };
 105
 106         if (throttle_value < DSHOT_THROTTLE_MIN) {
 107                 throttle_value = DSHOT_THROTTLE_MIN;
 108         }
 109
 110         if (throttle_value > DSHOT_THROTTLE_MAX) {
 111                 throttle_value = DSHOT_THROTTLE_MAX;
 112         }
 113
 114         // ...packets are the same for bidirectional mode
 115         dshot_rmt_packet.throttle_value = throttle_value;
 116         dshot_rmt_packet.telemetric_request = telemetric_request;
 117         dshot_rmt_packet.checksum = this->calc_dshot_chksum(dshot_rmt_packet);
 118
 119         output_rmt_data(dshot_rmt_packet);
 120 }
 121
 122 rmt_item32_t* DShotRMT::encode_dshot_to_rmt(uint16_t parsed_packet) {
 123     // ...is bidirecional mode activated
 124     if (bidirectional) {
 125         // ..."invert" the signal duration
 126         for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1)  {
 127                     if (parsed_packet & 0b1000000000000000) {
 128                             // set one
 129                             dshot_tx_rmt_item[i].duration0 = ticks_one_low;
 130                             dshot_tx_rmt_item[i].duration1 = ticks_one_high;
 131                     }
 132                     else {
 133                             // set zero
 134                             dshot_tx_rmt_item[i].duration0 = ticks_zero_low;
 135                             dshot_tx_rmt_item[i].duration1 = ticks_zero_high;
 136                     }
 137
 138                         dshot_tx_rmt_item[i].level0 = LOW;
 139                         dshot_tx_rmt_item[i].level1 = HIGH;
 140         }
 141     }
 142
 143     // ..."normal" DShot mode / "bidirectional" mode OFF
 144     else {
 145         for (int i = 0; i < DSHOT_PAUSE_BIT; i++, parsed_packet <<= 1)  {
 146                     if (parsed_packet & 0b1000000000000000) {
 147                             // set one
 148                             dshot_tx_rmt_item[i].duration0 = ticks_one_high;
 149                             dshot_tx_rmt_item[i].duration1 = ticks_one_low;
 150                     }
 151                     else {
 152                             // set zero
 153                             dshot_tx_rmt_item[i].duration0 = ticks_zero_high;
 154                             dshot_tx_rmt_item[i].duration1 = ticks_zero_low;
 155                     }
 156
 157                         dshot_tx_rmt_item[i].level0 = HIGH;
 158                         dshot_tx_rmt_item[i].level1 = LOW;
 159         }
 160     }
 161
 162         return dshot_tx_rmt_item;
 163 }
 164
 165 // ...just returns the checksum
 166 // DOES NOT APPEND CHECKSUM!!!
 167 uint16_t DShotRMT::calc_dshot_chksum(const dshot_packet_t& dshot_packet) {
 168         // ...same initial 12bit data for bidirectional or "normal" mode
 169         uint16_t packet = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request;
 170
 171         if (bidirectional) {
 172                 // ...calc the checksum "inverted" / bidirectional mode
 173                 return (~(packet ^ (packet >> 4) ^ (packet >> 8))) & 0x0F;
 174         } else {
 175                 // ...calc the checksum "normal" mode
 176                 return (packet ^ (packet >> 4) ^ (packet >> 8)) & 0x0F;
 177         }
 178 }
 179
 180 uint16_t DShotRMT::prepare_rmt_data(const dshot_packet_t& dshot_packet) {
 181         auto chksum = calc_dshot_chksum(dshot_packet);
 182
 183     // ..."construct" the packet
 184         uint16_t prepared_to_encode = (dshot_packet.throttle_value << 1) | dshot_packet.telemetric_request;
 185         prepared_to_encode = (prepared_to_encode << 4) | chksum;
 186
 187         return prepared_to_encode;
 188 }
 189
 190 // ...finally output using ESP32 RMT
 191 void DShotRMT::output_rmt_data(const dshot_packet_t& dshot_packet) {
 192         encode_dshot_to_rmt(prepare_rmt_data(dshot_packet));
 193
 194         rmt_tx_stop(rmt_channel);
 195         rmt_fill_tx_items(rmt_channel, dshot_tx_rmt_item, DSHOT_PACKET_LENGTH, 0);
 196         rmt_tx_start(rmt_channel, true);
 197 }
 198 #endif