Update Dshot timing and add eRPM telemetry support

Adjusted Dshot and Bidirectional Dshot PIO timing parameters for improved ESC compatibility. Added eRPM telemetry support for bidirectional Dshot outputs, including tracking and reporting eRPM values in the Out class and CLI output.

Author: qqqlab

src/cli/cli.cpp

@@ -202,6 +202,9 @@ static void cli_pout() {
   for(int i=0;i<OUT_SIZE;i++) {
     if(out.getType(i)) {
       Serial.printf("%c%d%%:%1.0f\t", out.getType(i), i, 100*out.get(i));
+      if(out.erpmEnabled[i]>=0) {
+        Serial.printf("erpm%d:%d\t", i, out.erpm[i]);
+      }
     }
   }
 }

src/hal/RP2040/Dshot/dshot_parallel.pio

@@ -26,11 +26,11 @@ SOFTWARE.
 
 .program dshot_parallel
 
-//A bit period (1667ns for Dshot600) is split in 8 equal parts. A zero-bit is encoded as 3H 5L (625ns high), and a one-bit is encoded as 6H 2L (1250ns high) - see: https://www.speedgoat.com/products/dshot
-//madflight v2.2.1: modified to zero: 5H 11L (520ns high), one: 12H 4L (1250ns high) as some ESC misinterpreted zeroes as ones
+//A bit period (1667ns for Dshot600) is split in 8 equal parts. A zero-bit is encoded as 3H 5L (625ns high), and a one-bit is encoded as 6H 2L (1250ns high) - see: https://brushlesswhoop.com/dshot-and-bidirectional-dshot/   https://blck.mn/2016/11/dshot-the-new-kid-on-the-block/  https://www.speedgoat.com/products/dshot
+//madflight v2.2.1: modified to zero: 1H 2L (556ns high), one: 2H 1L (1111ns high) as some ESC misinterpreted zeroes as ones
 .define public T1 5
-.define public T2 7
-.define public T3 4
+.define public T2 5
+.define public T3 5
 
 .wrap_target
     out x, 8

src/hal/RP2040/Dshot/dshot_parallel.pio.h

@@ -17,15 +17,15 @@
 #define dshot_parallel_pio_version 0
 
 #define dshot_parallel_T1 5
-#define dshot_parallel_T2 7
-#define dshot_parallel_T3 4
+#define dshot_parallel_T2 5
+#define dshot_parallel_T3 5
 
 static const uint16_t dshot_parallel_program_instructions[] = {
             //     .wrap_target
     0x6028, //  0: out    x, 8                       
     0xa40b, //  1: mov    pins, !null            [4] 
-    0xa601, //  2: mov    pins, x                [6] 
-    0xa203, //  3: mov    pins, null             [2] 
+    0xa401, //  2: mov    pins, x                [4] 
+    0xa303, //  3: mov    pins, null             [3] 
             //     .wrap
 };
 

src/hal/RP2040/DshotBidir/DshotBidirPio.h

@@ -58,7 +58,7 @@ The ESC replies with a 21 bit reply, the reply bit rate is 25% higher (5/4) than
 See https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ for full details on decoding.
 */
 
-#define DSHOT_DMA_BUF_SIZE (256 / 4) 
+#define DSHOT_DMA_BUF_SIZE (256 / 4) // buffer size in 32 bit words, one sample per byte
 //NOTE: dma buffer could be smaller, which will give faster rates but with risk of missing replies...
 //dma sampling period for 256 samples with 5.33x oversampling is: 256 / 5.33 * 4/5 Tbit = 39 Tbit
 //total period is: 16 Tbit (TX) + 2 Tbit (delay) + 39 Tbit (RX) + 10 Tbit (extra) =  67 Tbit
@@ -100,7 +100,7 @@ class DshotBidirPio {
         this->rate_khz = rate_khz;
         this->interval_us = (16 + 2 + 39 + 10) * 1000 / rate_khz; //16 (tx) + 2 (delay) + 39 (dma) + 10 (extra) -- FIXME: calculate dma delay based on DSHOT_DMA_BUF_SIZE and OSR
 
-        for(int i=0;i<DSHOT_DMA_BUF_SIZE;i++) dma_buf[i] = 0xffffffff; //init to high level, i.e. "no bidir reply"
+        for(int i = 0; i < DSHOT_DMA_BUF_SIZE; i++) dma_buf[i] = 0xffffffff; //init to high level, i.e. "no bidir reply"
 
         for(int i = 0 ; i < pin_count; i++) {
             gpio_pull_up(pin + i);
@@ -126,12 +126,12 @@ class DshotBidirPio {
         channel_config_set_read_increment(&c, false);
         channel_config_set_dreq(&c, pio_get_dreq(pio, sm, false));
         dma_channel_configure(
-            dma_ch,        // Channel to be configured
-            &c,            // The configuration we just created
-            NULL,           // The initial write address - set when starting dma
-            &(pio->rxf[sm]),           // The read address
+            dma_ch,             // Channel to be configured
+            &c,                 // The configuration we just created
+            NULL,               // The initial write address - set when starting dma
+            &(pio->rxf[sm]),    // The read address
             DSHOT_DMA_BUF_SIZE, // Number of transfers; in this case each is 1 byte.
-            false           // do not start
+            false               // do not start
         );
 
         setup_done = true;
@@ -182,15 +182,15 @@ class DshotBidirPio {
             frames[ch] = f;
         }
 
-        //convert frames into fifo stream
+        //convert frames into fifo stream (16 bytes, first byte is first frame bit for 8 channels)
         uint32_t d[4] = {};
-        for(int ch = 0; ch < pin_count; ch++) {
-            for(int bit = 0; bit < 16; bit++) {
+        for(int bit = 15; bit >= 0; bit--) {
+            uint32_t word = (15 - bit) / 4;
+            uint32_t shift = ((15 - bit) % 4) * 8; 
+            for(int ch=0; ch < pin_count; ch++) {
+                uint32_t chmask = (1u << ch);
                 if( frames[ch] & (1u << bit) ) {
-                    uint byte = 15 - bit; //byte in fifo stream (MSB of frame is sent first)
-                    uint32_t word = byte / 4; //32bit word in fifo stream
-                    uint32_t shift = (byte % 4) * 8; //shift in word (LSB of fifo is sent first)
-                    d[word] |= (1u << shift);
+                    d[word] |= (chmask << shift);
                 }
             }
         }
@@ -222,8 +222,8 @@ class DshotBidirPio {
     int read_erpm(uint8_t channel) {
         uint32_t tlm_val;
         int tlm_type = read_telem(channel, &tlm_val);
-        if(tlm_type<0) return tlm_type; //no data
-        if(tlm_type>0) return -100 - tlm_type; //data received, but not erpm
+        if(tlm_type < 0) return tlm_type; //no data
+        if(tlm_type > 0) return -100 - tlm_type; //data received, but not erpm
         return tlm_val;
     }
 
@@ -247,51 +247,50 @@ class DshotBidirPio {
         if(dma_channel_is_busy(dma_ch)) return -3;
 
         uint8_t *byte_buf = (uint8_t *)dma_buf;
-        uint8_t mask = 1<<channel;
+        uint8_t mask = 1 << channel;
 
-        /*
-        for(int i=0;i<DSHOT_DMA_BUF_SIZE*4;i++) {
+        /* print dma buffer
+        Serial.printf("ch%d:", channel);
+        for(int i = 0; i < DSHOT_DMA_BUF_SIZE * 4; i++) {
             uint8_t bit = (byte_buf[i] & mask ? 0 : 1); //bit value
             Serial.print(bit ? '1' : '-');
         }
-        Serial.println();
-        */
+        Serial.println(); 
+        //*/
 
         //over sampling rates * 100
         int osr100 = (dshot_bidir_T1 + dshot_bidir_T2 + dshot_bidir_T3) * 100 * 32 / 40 / dshot_bidir_TRX;
         int osr100off = osr100 * 6 / 10;  //should be 50% in theory, but 60% boosts shorter pulses a bit which appears to help decoding
-        //Serial.printf("osr100=%d ", osr100);
 
         uint8_t bit_last = 0;
         int pulse_len = 0;
         int bit_cnt = -1;
         uint32_t reply = 0;
 
-        for(int i=0;i<DSHOT_DMA_BUF_SIZE*4;i++) {
+        for(int i = 0; i < DSHOT_DMA_BUF_SIZE * 4; i++) {
             pulse_len++;
             uint8_t bit = (byte_buf[i] & mask ? 0 : 1); //bit value
             if(bit != bit_last) {
                 int bits = (pulse_len * 100 + osr100off) / osr100;
                 if(bits == 0) bits = 1; //accept short pulses as single bit
-                //Serial.printf("bit=%d len=%d bits=%d reply=%X bit_cnt=%d\n", bit_last, pulse_len, bits, reply, bit_cnt);
                 if(bit_cnt < 0) {
                     bit_cnt = 0;
                 }else{
-                    while(bit_cnt<21 && bits > 0) {
+                    while(bit_cnt < 21 && bits > 0) {
                         reply = (reply << 1) | bit_last; //store bits
                         bit_cnt++;
                         bits--;
                     }
                 }
                 pulse_len = 0;
                 bit_last = bit;
-                //Serial.printf("reply=%X bit_cnt=%d\n", reply, bit_cnt);
-            }               
-            if(bit_cnt>=21) break;
+            }
+            if(bit_cnt >= 21) break;
         }
 
-        if(bit_cnt<21) reply <<= 21 - bit_cnt; //append zeroes
+        if(bit_cnt < 21) reply <<= 21 - bit_cnt; //append zeroes
 
+        //Serial.printf("ch=%d reply=%X\n", channel, reply);
         *tlm_val = 0;
         return decode_telem(reply, tlm_val);
     }

src/hal/RP2040/DshotBidir/dshot_bidir.pio

@@ -33,9 +33,12 @@ SOFTWARE.
 
 ; one TX bit is 40 cycles, one RX bit is 32 cycles
 
-.define public T1 15
-.define public T2 15
-.define public T3 10
+//A bit period (1667ns for Dshot600) is split in 8 equal parts. A zero-bit is encoded as 3H 5L (625ns high), and a one-bit is encoded as 6H 2L (1250ns high) - see: https://brushlesswhoop.com/dshot-and-bidirectional-dshot/   https://blck.mn/2016/11/dshot-the-new-kid-on-the-block/  https://www.speedgoat.com/products/dshot
+//madflight v2.2.1: modified to zero: 1H 2L (556ns high), one: 2H 1L (1111ns high) as some ESC misinterpreted zeroes as ones  (new timing is approximate, kept 40 cycle TX bit timing)
+
+.define public T1 13
+.define public T2 14
+.define public T3 13
 
 //.define public TRX 8            ; 4x oversampling (response is 21 bits -> 81 samples)
 .define public TRX 6            ; 5.33x oversampling (response is 21 bits -> 112 samples)
@@ -46,22 +49,18 @@ txloop:
     mov pins, null   [T1-1]
     mov pins, x      [T2-1]
     mov pins, !null  [T3-3]
-    jmp !osre txloop            ; loop until ORS is empty
+    jmp !osre txloop            ; loop until OSR FIFO is empty
 
-    ; delay [31] to partially bridge gap between sender - receiver (nominal gap is 30 us, actual anything between 20 ~ 40 us)
+    ; Note: next instructions have delay [31] to partially bridge gap between sender - receiver (nominal gap is 30 us, measured between 20 ~ 40 us)
 
     mov osr, null    [31]      ; (*1) Fill OSR with zeroes
-    out pindirs, 32  [31]      ; (*1) Set pins to input with pullup (and empty osr)
+    out pindirs, 32  [31]      ; (*1) Set pins to input with pullup (and empty osr by pulling 32 bits)
 
 .wrap_target
     in pins, 8 [TRX-1]         ; keep on sampling 8 channel bytes until reset by C program
 .wrap
 
-; REDUCING NUMBER OF INSTRUCTIONS FROM 8 to 7 (version 1)
-
-; (*1) with PIO version 1 we can optimize "mov osr, null; out pindirs, 32" to "mov pindirs, null"
-
-
+; (*1) Note: with PIO version 1 we can optimize "mov osr, null; out pindirs, 32" to "mov pindirs, null"
 
 
 % c-sdk {
@@ -91,74 +90,3 @@ static inline void dshot_bidir_program_init(PIO pio, uint sm, uint offset, uint
     pio_sm_clear_fifos(pio, sm); //just to be sure
 }
 %}
-
-
-
-;=================================================================================================
-; 1-8 channel Bidirectional DSHOT, needs blocking/isr/dma to bytes with 8-channel reply samples from fifo
-;=================================================================================================
-
-.program dshot_bidir8
-
-; one TX bit is 40 cycles, one RX bit is 32 cycles
-
-.define public T1 15
-.define public T2 15
-.define public T3 10
-
-//.define public TRX 8            ; 4x oversampling (response is 21 bits -> 81 samples)
-.define public TRX 6            ; 5.33x oversampling (response is 21 bits -> 112 samples)
-//.define public TRX 5            ; 6.4x oversampling (response is 21 bits -> 135 samples)
-
-
-txloop:
-    out x, 8                    ; "out x" is needed to stall here and keep pins high when fifo is empty (I.e. can't use "out pins 8" instead of "out x 8; mov pins x")
-    mov pins, null   [T1-1]
-    mov pins, x      [T2-1]
-    mov pins, !null  [T3-3]
-    jmp !osre txloop            ; loop until ORS is empty
-
-    ; delay [31] to partially bridge gap between sender - receiver (nominal gap is 30 us, actual anything between 20 ~ 40 us)
-
-    mov osr, null    [31]      ; (*1) Fill OSR with zeroes
-    out pindirs, 32  [31]      ; (*1) Set pins to input with pullup (and empty osr)
-
-    ; NOTE: we can't wait for a pin, as we would have to monitor 8 pins, C program discards samples until first 0
-
-.wrap_target
-    in pins 8 [TRX-1]          ; keep on sampling until reset by C program
-.wrap
-
-
-; REDUCING NUMBER OF INSTRUCTIONS FROM 7 to 6 (version 1 only)
-
-; (*1) with PIO version 1 we can optimize "mov osr, null; out pindirs, 32" to "mov pindirs, null"
-
-% c-sdk {
-#include "hardware/clocks.h"
-
-// init the sm in disabled state, ready for loading tx fifo.
-static inline void dshot_bidir8_program_init(PIO pio, uint sm, uint offset, uint pin_base, uint pin_count, float freq) {
-    pio_sm_set_enabled(pio, sm, false); //disable sm
-
-    for(uint i=pin_base; i<pin_base+pin_count; i++) {
-        pio_gpio_init(pio, i);
-    }
-    pio_sm_set_consecutive_pindirs(pio, sm, pin_base, pin_count, true); //set output
-
-    pio_sm_config c = dshot_bidir8_program_get_default_config(offset);
-    sm_config_set_out_shift(&c, true, true, 32); //autopull
-    sm_config_set_out_pins(&c, pin_base, pin_count);
-    
-    sm_config_set_in_shift(&c, true, true, 32); //autopush
-    sm_config_set_in_pins(&c, pin_base);
-
-    int cycles_per_bit = dshot_bidir8_T1 + dshot_bidir8_T2 + dshot_bidir8_T3;
-    float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit);
-    sm_config_set_clkdiv(&c, div);
-
-    pio_sm_init(pio, sm, offset, &c);
-
-    pio_sm_clear_fifos(pio, sm); //just to be sure
-}
-%}

src/hal/RP2040/DshotBidir/dshot_bidir.pio.h

@@ -16,16 +16,16 @@
 #define dshot_bidir_wrap 7
 #define dshot_bidir_pio_version 0
 
-#define dshot_bidir_T1 15
-#define dshot_bidir_T2 15
-#define dshot_bidir_T3 10
+#define dshot_bidir_T1 13
+#define dshot_bidir_T2 14
+#define dshot_bidir_T3 13
 #define dshot_bidir_TRX 6
 
 static const uint16_t dshot_bidir_program_instructions[] = {
     0x6028, //  0: out    x, 8                       
-    0xae03, //  1: mov    pins, null             [14]
-    0xae01, //  2: mov    pins, x                [14]
-    0xa70b, //  3: mov    pins, !null            [7] 
+    0xac03, //  1: mov    pins, null             [12]
+    0xad01, //  2: mov    pins, x                [13]
+    0xaa0b, //  3: mov    pins, !null            [10]
     0x00e0, //  4: jmp    !osre, 0                   
     0xbfe3, //  5: mov    osr, null              [31]
     0x7f80, //  6: out    pindirs, 32            [31]
@@ -73,68 +73,3 @@ static inline void dshot_bidir_program_init(PIO pio, uint sm, uint offset, uint
 
 #endif
 
-// ------------ //
-// dshot_bidir8 //
-// ------------ //
-
-#define dshot_bidir8_wrap_target 7
-#define dshot_bidir8_wrap 7
-#define dshot_bidir8_pio_version 0
-
-#define dshot_bidir8_T1 15
-#define dshot_bidir8_T2 15
-#define dshot_bidir8_T3 10
-#define dshot_bidir8_TRX 6
-
-static const uint16_t dshot_bidir8_program_instructions[] = {
-    0x6028, //  0: out    x, 8                       
-    0xae03, //  1: mov    pins, null             [14]
-    0xae01, //  2: mov    pins, x                [14]
-    0xa70b, //  3: mov    pins, !null            [7] 
-    0x00e0, //  4: jmp    !osre, 0                   
-    0xbfe3, //  5: mov    osr, null              [31]
-    0x7f80, //  6: out    pindirs, 32            [31]
-            //     .wrap_target
-    0x4508, //  7: in     pins, 8                [5] 
-            //     .wrap
-};
-
-#if !PICO_NO_HARDWARE
-static const struct pio_program dshot_bidir8_program = {
-    .instructions = dshot_bidir8_program_instructions,
-    .length = 8,
-    .origin = -1,
-    .pio_version = 0,
-#if PICO_PIO_VERSION > 0
-    .used_gpio_ranges = 0x0
-#endif
-};
-
-static inline pio_sm_config dshot_bidir8_program_get_default_config(uint offset) {
-    pio_sm_config c = pio_get_default_sm_config();
-    sm_config_set_wrap(&c, offset + dshot_bidir8_wrap_target, offset + dshot_bidir8_wrap);
-    return c;
-}
-
-#include "hardware/clocks.h"
-// init the sm in disabled state, ready for loading tx fifo.
-static inline void dshot_bidir8_program_init(PIO pio, uint sm, uint offset, uint pin_base, uint pin_count, float freq) {
-    pio_sm_set_enabled(pio, sm, false); //disable sm
-    for(uint i=pin_base; i<pin_base+pin_count; i++) {
-        pio_gpio_init(pio, i);
-    }
-    pio_sm_set_consecutive_pindirs(pio, sm, pin_base, pin_count, true); //set output
-    pio_sm_config c = dshot_bidir8_program_get_default_config(offset);
-    sm_config_set_out_shift(&c, true, true, 32); //autopull
-    sm_config_set_out_pins(&c, pin_base, pin_count);
-    sm_config_set_in_shift(&c, true, true, 32); //autopush
-    sm_config_set_in_pins(&c, pin_base);
-    int cycles_per_bit = dshot_bidir8_T1 + dshot_bidir8_T2 + dshot_bidir8_T3;
-    float div = clock_get_hz(clk_sys) / (freq * cycles_per_bit);
-    sm_config_set_clkdiv(&c, div);
-    pio_sm_init(pio, sm, offset, &c);
-    pio_sm_clear_fifos(pio, sm); //just to be sure
-}
-
-#endif
-