PICO: DSHOT non-bidir and work in progress.

src/platform/PICO/dshot.pio

@@ -0,0 +1,87 @@
+;
+; Copyright (c) TODO
+;
+
+; DSHOT protocol rate (150, 300, 600) is dynamic
+; DSHOT bidir / not bidir is compiled in
+; For rate, could have separate programs ready to load in (maybe easiest)
+;  or could have effectively dynamic based on contents of x register if spare, which can be
+; set with pio_sm_exec_wait_blocking
+; For >4 motors, could  configure pin with disable, config set, enable, if blocking
+; 
+
+; presumably BF writes dshot async, at max of ~10k/sec or so (typical aiming for 8k loop for gyro)
+; dshot frame takes approx. 0.1ms dshot150 or dshot300_bidir
+; so hopefully never going to be overflowing the tx fifo (but could happen with 150bidir and be close with 150, 300bidir)
+
+
+; programs dshot_150, dshot_300, dshot_600, dshot_bidir_150, dshot_bidir_300, dshot_bidir_600
+;
+; All programs, bit width = 40 cycles, clock set accordingly
+; Min delay between frames of 2us, number of cycles varies according to program
+;
+; DSHOT 600
+; 1 bit ~ 1.667us, 2us ~ 48 cycles. Allow say 56 cycles between frames
+;
+
+; * could get rid of nop by adding delays to 2x jmp !osre, bitloop
+; * maybe get rid of discard by writing 16-bit word (dupllicated to top 16 bits), and osre empty after 16 bits
+
+.program dshot_600
+start:
+    set    pins, 0                [31]      ; Set pin low [assumed pin already set for output]
+    nop                           [20]      ; min delay 2us at DSHOT600 is 48 cycles, allow 56 before next set
+    pull   block                            ; Block until someone puts something into the TX FIFO
+    out    y, 16                            ; Discard top 16 bits
+bitloop:
+    out    y, 1                             ; Shift next bit into y
+    jmp    !y, outzero                      ;
+    set    pins, 1                [29]      ; To output a '1' bit, set high for 30 then low for 10 (based on bitlength = 40 cycles)
+    set    pins, 0                [6]       ; Delay to next "set" adds up to 10
+    jmp    !osre, bitloop                   ; If not done output (finished shifting bits) the loop
+    jmp    start
+outzero:
+    set    pins, 1                [14]      ; To output a '1', set high for 15 then low for 25
+    set    pins, 0                [20]      ; Delay to next "set" adds up to 25
+    jmp    !osre, bitloop         [1]       ; If not done output (finished shifting bits) the loop else wrap to start
+
+
+
+/*
+
+#if 0 //def TEST_DSHOT_ETC
+
+static const uint16_t dshot_bidir_PIO_instructions[] = {
+            //     .wrap_target
+    0xe081, //  0: set    pindirs, 1
+    0xea00, //  1: set    pins, 0                [10]
+    0xe501, //  2: set    pins, 1                [5]
+    0xea00, //  3: set    pins, 0                [10]
+    0xe501, //  4: set    pins, 1                [5]
+    0x0001, //  5: jmp    1
+            //     .wrap
+};
+
+/*
+#if !PICO_NO_HARDWARE
+static const struct pio_program blink_program = {
+    .instructions = blink_program_instructions,
+    .length = 6,
+    .origin = -1,
+    .pio_version = blink_pio_version,
+#if PICO_PIO_VERSION > 0
+    .used_gpio_ranges = 0x0
+#endif
+};
+
+#define blink_wrap_target 0
+#define blink_wrap 5
+#define blink_pio_version 0
+
+static inline pio_sm_config blink_program_get_default_config(uint offset) {
+    pio_sm_config c = pio_get_default_sm_config();
+    sm_config_set_wrap(&c, offset + blink_wrap_target, offset + blink_wrap);
+    return c;
+}
+*/
+

src/platform/PICO/dshot.pio.programs

@@ -0,0 +1,54 @@
+// -------------------------------------------------- //
+// This file is autogenerated by pioasm; do not edit! //
+// -------------------------------------------------- //
+
+#pragma once
+
+#if !PICO_NO_HARDWARE
+#include "hardware/pio.h"
+#endif
+
+// --------- //
+// dshot_600 //
+// --------- //
+
+#define dshot_600_wrap_target 0
+#define dshot_600_wrap 12
+#define dshot_600_pio_version 0
+
+static const uint16_t dshot_600_program_instructions[] = {
+            //     .wrap_target
+    0xff00, //  0: set    pins, 0                [31]
+    0xb442, //  1: nop                           [20]
+    0x80a0, //  2: pull   block
+    0x6050, //  3: out    y, 16
+    0x6041, //  4: out    y, 1
+    0x006a, //  5: jmp    !y, 10
+    0xfd01, //  6: set    pins, 1                [29]
+    0xe600, //  7: set    pins, 0                [6]
+    0x00e4, //  8: jmp    !osre, 4
+    0x0000, //  9: jmp    0
+    0xee01, // 10: set    pins, 1                [14]
+    0xf400, // 11: set    pins, 0                [20]
+    0x01e4, // 12: jmp    !osre, 4               [1]
+            //     .wrap
+};
+
+#if !PICO_NO_HARDWARE
+static const struct pio_program dshot_600_program = {
+    .instructions = dshot_600_program_instructions,
+    .length = 13,
+    .origin = -1,
+    .pio_version = dshot_600_pio_version,
+#if PICO_PIO_VERSION > 0
+    .used_gpio_ranges = 0x0
+#endif
+};
+
+static inline pio_sm_config dshot_600_program_get_default_config(uint offset) {
+    pio_sm_config c = pio_get_default_sm_config();
+    sm_config_set_wrap(&c, offset + dshot_600_wrap_target, offset + dshot_600_wrap);
+    return c;
+}
+#endif
+

src/platform/PICO/dshot_pico.c

@@ -45,14 +45,19 @@
 #include "hardware/pio.h"
 #include "hardware/clocks.h"
 
+////////FAST_DATA_ZERO_INIT timeUs_t dshotFrameUs;
+//static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
+
+#ifdef USE_DSHOT_TELEMETRY
 // Maximum time to wait for telemetry reception to complete
 #define DSHOT_TELEMETRY_TIMEOUT 2000
 
-FAST_DATA_ZERO_INIT timeUs_t dshotFrameUs;
+// TODO use with TELEMETRY for cli report (or not)
-//static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
+FAST_DATA_ZERO_INIT dshotTelemetryCycleCounters_t dshotDMAHandlerCycleCounters;
+#endif
 
 typedef struct motorOutput_s {
-    int offset;
+    int offset;              // NB current code => offset same for all motors
     dshotProtocolControl_t protocolControl;
     int pinIndex;            // pinIndex of this motor output within a group that bbPort points to
     IO_t io;                 // IO_t for this output
@@ -74,45 +79,6 @@ static motorProtocolTypes_e motorProtocol;
 static motorOutput_t dshotMotors[MAX_SUPPORTED_MOTORS];
 bool useDshotTelemetry = false;
 
-#define DSHOT_BIT_PERIOD 40
-
-static const uint16_t dshot_bidir_PIO_instructions[] = {
-            //     .wrap_target
-    0xff81, //  0: set    pindirs, 1             [31]
-    0xff01, //  1: set    pins, 1                [31]
-    0x80a0, //  2: pull   block
-    0x6050, //  3: out    y, 16
-    0x00e6, //  4: jmp    !osre, 6
-    0x000e, //  5: jmp    14
-    0x6041, //  6: out    y, 1
-    0x006b, //  7: jmp    !y, 11
-    0xfd00, //  8: set    pins, 0                [29]
-    0xe501, //  9: set    pins, 1                [5]
-    0x0004, // 10: jmp    4
-    0xee00, // 11: set    pins, 0                [14]
-    0xf401, // 12: set    pins, 1                [20]
-    0x0004, // 13: jmp    4
-    0xe055, // 14: set    y, 21
-    0x1f8f, // 15: jmp    y--, 15                [31]
-    0xe080, // 16: set    pindirs, 0
-    0xe05f, // 17: set    y, 31
-    0xa142, // 18: nop                           [1]
-    0x0076, // 19: jmp    !y, 22
-    0x0095, // 20: jmp    y--, 21
-    0x00d2, // 21: jmp    pin, 18
-    0xe05e, // 22: set    y, 30
-    0x4901, // 23: in     pins, 1                [9]
-    0x0097, // 24: jmp    y--, 23
-    0x4801, // 25: in     pins, 1                [8]
-    0x8020, // 26: push   block
-    0xe05f, // 27: set    y, 31
-    0x4a01, // 28: in     pins, 1                [10]
-    0x009c, // 29: jmp    y--, 28
-    0x8020, // 30: push   block
-    0x0000, // 31: jmp    0
-            //     .wrap
-};
-
 static float getPeriodTiming(void)
 {
     switch(motorProtocol) {
@@ -126,44 +92,237 @@ static float getPeriodTiming(void)
     }
 }
 
+#define DSHOT_BIT_PERIOD 40
+
+#ifdef USE_DSHOT_TELEMETRY
+// Program for bidirectional ("inverted") DSHOT,
+// 1 is low, 0 is high voltage
+// Outgoing frame starts with transition from 0 to 1
+// then 16x bits of 1 == 1 for ~2/3 period, 0 for ~1/3
+//               or 0 == 1 for ~1/3 period, 0 for ~2/3
+// Bit period is DSHOT_BIT_PERIOD (currently 40 cycles)
+// Expect the return frame at 30 us after end of outgoing frame
+//  TODO 30us independent of DSHOT 150, 300, 600,
+//       so need different number of cycles delay for DSHOT 600 vs 300 vs 150
+// Return frame is encoded as 21 bits (simple lo = 0, hi = 1), at 5/4 x outgoing bitrate
+// (so return bit period should currently be 40 * 4 / 5 = 32 cycles)
+
+// 1st bit of return frame is always 0, "idle" is 1, so after ~25 us could start waiting for transition
+// to 0 - when we finish the out frame, we set pin to input (pindirs 0). Will that respect a previous
+// pullup setting?
+// see https://forums.raspberrypi.com/viewtopic.php?t=311659 
+
+/*
+comment on syncing (?) for return frame
+https://github.com/betaflight/betaflight/pull/8554
+ledvinap commented on Jul 16, 2019
+A side note: I didn't analyze it too far, but is seems that 1.5x oversampling (1.5 synchronous samples per bit period) should be enough to handle NRZ protocol including clock drift...
+    
+joelucid commented on Jul 17, 2019 • 
+Let me specify the new rpm telemetry protocol here for you @sskaug, and others:
+
+Dshot bidir uses inverted signal levels (idle is 1). FC to ESC uses dshot frames but the lowest 4 bits hold the complement of the other nibbles xor'd together (normal dshot does not complement the xor sum). The ESC detects based on the inversion that telemetry packets have to be sent.
+
+30us after receiving the dshot frame the esc responds with a telemetry frame. Logically the telemetry frame is a 16 bit value and the lowest 4 bits hold the complemented xor'd nibbles again.
+
+The upper 12 bit contain the eperiod (1/erps) in the following bitwise encoding:
+
+e e e m m m m m m m m m
+The 9 bit value M needs to shifted left E times to get the period in micro seconds. This gives a range of 1 us to 65408 us. Which translates to a min e-frequency of 15.29 hz or for 14 pole motors 3.82 hz.
+
+This 16 bit value is then GCR encoded to a 20 bit value by applying the following map nibble-wise:
+
+0 -> 19
+1 -> 1b
+2 -> 12
+3 -> 13
+4 -> 1d
+5 -> 15
+6 -> 16
+7 -> 17
+8 -> 1a
+9 -> 09
+a -> 0a
+b -> 0b
+c -> 1e
+d -> 0d
+e -> 0e
+f -> 0f 
+This creates a 20 bit value which has no more than two consecutive zeros. This value is mapped to a new 21 bit value by starting with a bit value of 0 and changing the bit value in the next bit if the current bit in the incoming value is a 1, but repeating the previous bit value otherwise. Example:
+
+1 0 1 1 1 0 0 1 1 0 would become 0 1 1 0 1 0 0 0 1 0 0.
+
+This 21 bit value is then sent uninverted at a bitrate of 5/4 * the dshot bitrate. So Dshot 3 uses 375 kbit, dshot 600 uses 750 kbit.
+
+The esc needs to be ready after 40us + one dshot frame length to receive the next dshot packet.
+
+See https://en.wikipedia.org/wiki/Run-length_limited#GCR:_(0,2)_RLL for more details on the GCR encoding.
+
+*/
+
+static const uint16_t dshot_bidir_PIO_instructions[] = {
+            //     .wrap_target
+    0xff81, //  0: set    pindirs, 1             [31]      // min delay... (TODO check, and is this correct for dshot != 600?
+    0xff01, //  1: set    pins, 1                [31]
+    0x80a0, //  2: pull   block                            // Block until someone puts something into the TX FIFO
+    0x6050, //  3: out    y, 16                            // discard top 16 bits (can lose this if 16-bit write? but then would have to count 16)
+    0x00e6, //  4: jmp    !osre, 6                         // If not done output then -> 6
+    0x000e, //  5: jmp    14
+    0x6041, //  6: out    y, 1                             // next bit -> into y
+    0x006b, //  7: jmp    !y, 11                           // 0 bit to output -> 11
+    0xfd00, //  8: set    pins, 0                [29]      // 1 bit to output. Inverted Dshot => 0 for 30, 1 for 10
+    0xe501, //  9: set    pins, 1                [5]
+    0x0004, // 10: jmp    4
+    0xee00, // 11: set    pins, 0                [14]      // 0 bit to output. Inverted Dshot => 0 for 15, 1 for 25
+    0xf401, // 12: set    pins, 1                [20]
+    0x0004, // 13: jmp    4
+    0xe055, // 14: set    y, 21                            // after end of output frame, wait ~30us for start of input which should be transition to 0 
+    0x1f8f, // 15: jmp    y--, 15                [31]      // wait 32*21 = 672 cycles = 16.8 bit periods ~ 28us at DSHOT600 rate
+    0xe080, // 16: set    pindirs, 0
+    0xe05f, // 17: set    y, 31
+    0xa142, // 18: nop                           [1]
+    0x0076, // 19: jmp    !y, 22
+    0x0095, // 20: jmp    y--, 21                             // TODO dead code?
+    0x00d2, // 21: jmp    pin, 18                             // TODO dead code?                          
+    0xe05e, // 22: set    y, 30
+    0x4901, // 23: in     pins, 1                [9]          
+    0x0097, // 24: jmp    y--, 23                               // retrive 31 bits? every 11 cycles (in bits are at 40*4/5 = 32 cycles for dshot600)
+    0x4801, // 25: in     pins, 1                [8]            // retrieve 32nd bit TODO we want 21 (or 20 given leading 0) - are we ~3x sampling?
+////////////    0x8020, // 26: push   block
+    0x8000, // 26: push   [not block]
+    0xe05f, // 27: set    y, 31
+    0x4a01, // 28: in     pins, 1                [10]           // retrieve another 32 bits at 11 cycles
+    0x009c, // 29: jmp    y--, 28
+////////////    0x8020, // 26: push   block
+    0x8000, // 26: push   [not block]
+//    0x8020, // 30: push   block        <---- problem if not expecting telemetry...?
+    0x0000, // 31: jmp    0                                   // can remove this one with wrap
+            //     .wrap
+};
+
 static const struct pio_program dshot_bidir_program = {
     .instructions = dshot_bidir_PIO_instructions,
     .length = ARRAYLEN(dshot_bidir_PIO_instructions),
     .origin = -1,
 };
 
-static void dshot_bidir_program_init(PIO pio, uint sm, int offset, uint pin)
+static void dshot_program_bidir_init(PIO pio, uint sm, int offset, uint pin)
 {
     pio_sm_config config = pio_get_default_sm_config();
-    sm_config_set_wrap(&config, offset + ARRAYLEN(dshot_bidir_PIO_instructions), offset);
+
+    // Wrap to = offset + 0, Wrap after instruction = offset + length - 1
+    sm_config_set_wrap(&config, offset, offset + ARRAYLEN(dshot_bidir_PIO_instructions) - 1);
 
     sm_config_set_set_pins(&config, pin, 1);
     sm_config_set_in_pins(&config, pin);
     sm_config_set_jmp_pin (&config, pin);
     pio_gpio_init(pio, pin);
-    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true);
+    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); // set pin to output
-    gpio_set_pulls(pin, true, false);
+
-    sm_config_set_out_shift(&config, false, false, ARRAYLEN(dshot_bidir_PIO_instructions));
+    gpio_set_pulls(pin, true, false); // Pull up - idle 1 when awaiting bidir telemetry input (PIO pindirs 0).
+
+
+// TODO ** autopull == false?
+    // TODO ** check shiftright = false for dshot program
+    // recall we aren't outputting the pulled byte, we are using it for timing of transition of output level
+    // from 0 back to 1
+    // remember also that DSHOT is effectively hi-endian w.r.t. time (so the timeline looks lo-endian
+    // in the usual way of picturing memory) -> so shift left
+
+    // TODO pull again at 16 bits, then don't need to discard 16 bits in PIO program
+    sm_config_set_out_shift(&config, false, false, 32); /////ARRAYLEN(dshot_bidir_PIO_instructions));
+// TODO ** autopush == false?
     sm_config_set_in_shift(&config, false, false, ARRAYLEN(dshot_bidir_PIO_instructions));
 
     float clocks_per_us = clock_get_hz(clk_sys) / 1000000;
+#ifdef TEST_DSHOT_SLOW
+    sm_config_set_clkdiv(&config, (1.0e4f + getPeriodTiming()) / DSHOT_BIT_PERIOD * clocks_per_us);
+#else
     sm_config_set_clkdiv(&config, getPeriodTiming() / DSHOT_BIT_PERIOD * clocks_per_us);
+#endif
 
     pio_sm_init(pio, sm, offset, &config);
 }
 
+#else
+
+static const uint16_t dshot_600_program_instructions[] = {
+            //     .wrap_target
+    0xff00, //  0: set    pins, 0                [31]
+    0xb442, //  1: nop                           [20]
+    0x80a0, //  2: pull   block
+    0x6050, //  3: out    y, 16
+    0x6041, //  4: out    y, 1
+    0x006a, //  5: jmp    !y, 10
+    0xfd01, //  6: set    pins, 1                [29]
+    0xe600, //  7: set    pins, 0                [6]
+    0x00e4, //  8: jmp    !osre, 4
+    0x0000, //  9: jmp    0
+    0xee01, // 10: set    pins, 1                [14]
+    0xf400, // 11: set    pins, 0                [20]
+    0x01e4, // 12: jmp    !osre, 4               [1]
+            //     .wrap
+};
+
+static const struct pio_program dshot_600_program = {
+    .instructions = dshot_600_program_instructions,
+    .length = ARRAYLEN(dshot_600_program_instructions),
+    .origin = -1,
+};
+
+// TODO DSHOT150, 300
+static void dshot_program_init(PIO pio, uint sm, int offset, uint pin)
+{
+    pio_sm_config config = pio_get_default_sm_config();
+
+    // Wrap to = offset + 0, Wrap after instruction = offset + length - 1
+    sm_config_set_wrap(&config, offset, offset + ARRAYLEN(dshot_600_program_instructions) - 1);
+
+    sm_config_set_set_pins(&config, pin, 1);
+    pio_gpio_init(pio, pin);
+    pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); // set pin to output
+    gpio_set_pulls(pin, false, true); // Pull down - idle low when awaiting next frame to output.
+
+    // TODO pull again at 16 bits, then don't need to discard 16 bits in PIO program
+    // write 16-bits -> duplicates (TBC)
+    sm_config_set_out_shift(&config, false, false, 32);
+    sm_config_set_fifo_join(&config, PIO_FIFO_JOIN_TX);
+
+    float clocks_per_us = clock_get_hz(clk_sys) / 1000000;
+#ifdef TEST_DSHOT_SLOW
+    sm_config_set_clkdiv(&config, (1.0e4f + getPeriodTiming()) / DSHOT_BIT_PERIOD * clocks_per_us);
+#else
+    sm_config_set_clkdiv(&config, getPeriodTiming() / DSHOT_BIT_PERIOD * clocks_per_us);
+#endif
+
+    pio_sm_init(pio, sm, offset, &config);
+}
+
+#endif
+
+
+/*
+TODO what actually finally fixed this? was it removing gpio_set_dir (unlikely) or
+    the whole IOConfigGPIO (which needs rethinking)?
+    TODO is Dshot writing async? (what about telemetry?)
+*/  
+    
+
+#ifdef USE_DSHOT_TELEMETRY
 static uint32_t decodeTelemetry(const uint32_t first, const uint32_t second)
 {
     UNUSED(first);
     UNUSED(second);
     return DSHOT_TELEMETRY_INVALID;
 }
+#endif
 
 static bool dshotTelemetryWait(void)
 {
+    bool telemetryWait = false;
+#ifdef USE_DSHOT_TELEMETRY
     // Wait for telemetry reception to complete
     bool telemetryPending;
-    bool telemetryWait = false;
     const timeUs_t startTimeUs = micros();
 
     do {
@@ -171,7 +330,7 @@ static bool dshotTelemetryWait(void)
         for (unsigned motorIndex = 0; motorIndex < dshotMotorCount && !telemetryPending; motorIndex++) {
             const motorOutput_t *motor = &dshotMotors[motorIndex];
             const int fifo_words = pio_sm_get_rx_fifo_level(motor->pio, motor->pio_sm);
-            telemetryPending |= fifo_words >= 2;
+            telemetryPending |= fifo_words >= 2; // TODO Current program outputs two words. Should this be <2 ?
         }
 
         telemetryWait |= telemetryPending;
@@ -179,13 +338,14 @@ static bool dshotTelemetryWait(void)
         if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
             break;
         }
-    } while (telemetryPending);
+    } while (telemetryPending); // TODO logic here? supposed to block until all telemetry available?
 
     if (telemetryWait) {
         DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
     }
 
-    return telemetryWait;
+#endif
+    return telemetryWait; // TODO what should the return value be? (don't think it's used)
 }
 
 static void dshotUpdateInit(void)
@@ -256,6 +416,7 @@ static void dshotWriteInt(uint8_t motorIndex, uint16_t value)
     if (dshotCommandIsProcessing()) {
         value = dshotCommandGetCurrent(motorIndex);
         if (value) {
+            // Request telemetry on other return wire (this isn't DSHOT bidir)
             motor->protocolControl.requestTelemetry = true;
         }
     }
@@ -263,7 +424,15 @@ static void dshotWriteInt(uint8_t motorIndex, uint16_t value)
     motor->protocolControl.value = value;
 
     uint16_t packet = prepareDshotPacket(&motor->protocolControl);
+
+// TODO what to do if TX buffer is full? (unlikely at standard scheduler loop rates?)
+#ifdef TEST_DSHOT_ETC
+    // for testing, can be convenient to block, not lose any writes
+    pio_sm_put_blocking(motor->pio, motor->pio_sm, packet);
+////    pio_sm_put(motor->pio, motor->pio_sm, packet);
+#else    
     pio_sm_put(motor->pio, motor->pio_sm, packet);
+#endif
 }
 
 static void dshotWrite(uint8_t motorIndex, float value)
@@ -273,6 +442,9 @@ static void dshotWrite(uint8_t motorIndex, float value)
 
 static void dshotUpdateComplete(void)
 {
+    // TODO: anything here?
+    // aargh, dshotCommandQueueEmpty is a query, but dshotCommandOutputIsEnabled has side-effects...
+
     // If there is a dshot command loaded up, time it correctly with motor update
     if (!dshotCommandQueueEmpty()) {
         if (!dshotCommandOutputIsEnabled(dshotMotorCount)) {
@@ -286,7 +458,13 @@ static bool dshotEnableMotors(void)
     for (int i = 0; i < dshotMotorCount; i++) {
         const motorOutput_t *motor = &dshotMotors[i];
         if (motor->configured) {
-            IOConfigGPIO(motor->io, 0);
+            // TODO is this valid on a PIO pin
+            // no, it only affects the pin when configured as "SIO"
+            // gpio_set_dir(ioPin, (cfg & 0x01)); // 0 = in, 1 = out
+            ///// not this now it inits the pin... IOConfigGPIO(motor->io, 0);
+
+            // if we want to set the direction in a PIO mode, we do
+            // pio_sm_set_consecutive_pindirs(pio, sm, pin, 1, true); // set pin to output
         }
     }
     return true;
@@ -294,11 +472,28 @@ static bool dshotEnableMotors(void)
 
 static void dshotDisableMotors(void)
 {
+    // TODO: implement?
     return;
 }
 
+#ifdef TEST_DSHOT_ETC
+void dshotTestWrites(void)
+{
+//    const int mnum = 0;
+    const int mval = DSHOT_MIN_THROTTLE;
+    for (int ii = mval; ii< DSHOT_MAX_THROTTLE; ii += 123) {
+//        dshotWriteInt(mnum, ii);
+        dshotWriteInt(0, ii);
+        dshotWriteInt(1, ii);
+        dshotWriteInt(2, ii);
+        dshotWriteInt(3, ii);
+    }
+}
+#endif
+
 static void dshotShutdown(void)
 {
+    // TODO: implement?
     return;
 }
 
@@ -324,7 +519,9 @@ static bool dshotIsMotorIdle(unsigned motorIndex)
 
 static void dshotRequestTelemetry(unsigned index)
 {
-#ifdef USE_DSHOT_TELEMETRY
+#ifndef USE_DSHOT_TELEMETRY
+    UNUSED(index);
+#else
     if (index < dshotMotorCount) {
         dshotMotors[index].protocolControl.requestTelemetry = true;
     }
@@ -351,55 +548,90 @@ static motorVTable_t dshotVTable = {
 
 bool dshotPwmDevInit(motorDevice_t *device, const motorDevConfig_t *motorConfig)
 {
+    // Return false if not enough motors initialised for the mixer or a break in the motors or issue with PIO
     dbgPinLo(0);
     dbgPinLo(1);
 
+    device->vTable = NULL; // Only set vTable if initialisation is succesful (TODO: check)
+    dshotMotorCount = 0;   // Only set dshotMotorCount ble if initialisation is succesful (TODO: check)
+
+    uint8_t motorCountProvisional = device->count;
+    if (motorCountProvisional > 4) {
+        // Currently support 4 motors with one PIO block, four state machines
+        // TODO (possible future) support more than 4 motors
+        // (by reconfiguring state machines perhaps? batching if required)
+        return false;
+    }
+
     motorProtocol = motorConfig->motorProtocol;
-    device->vTable = &dshotVTable;
-    dshotMotorCount = device->count;
 
 #ifdef USE_DSHOT_TELEMETRY
     useDshotTelemetry = motorConfig->useDshotTelemetry;
 #endif
 
-    for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < dshotMotorCount; motorIndex++) {
+    // TODO somehow configure which PIO block to use for dshot? pio0 for now.
-        IO_t io = IOGetByTag(motorConfig->ioTags[motorIndex]);
+    const PIO pio = pio0;
 
-        const PIO pio = pio0;
+#ifdef TEST_DSHOT_ETC
+    pio_set_gpio_base(pio, 16);
+#endif
+    
+    // Use one program for all motors.
+    // NB the PIO block is limited to 32 instructions (shared across 4 state machines)
+#ifdef USE_DSHOT_TELEMETRY
+    int offset = pio_add_program(pio, &dshot_bidir_program);
+#else
+    int offset = pio_add_program(pio, &dshot_600_program);
+#endif
+    if (offset < 0) {
+        /* error loading PIO */
+        return false;
+    }
+
+    for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCountProvisional; motorIndex++) {
+        IO_t io = IOGetByTag(motorConfig->ioTags[motorIndex]);
+        if (!IOIsFreeOrPreinit(io)) {
+            return false;
+        }
+
+        // TODO: might make use of pio_claim_free_sm_and_add_program_for_gpio_range
+        // -> automatically sets the GPIO base if we might be using pins in 32..47
         const int pio_sm = pio_claim_unused_sm(pio, false);
 
-        if (!IOIsFreeOrPreinit(io) || pio_sm < 0) {
+        if (pio_sm < 0) {
-            /* not enough motors initialised for the mixer or a break in the motors or issue with PIO */
-            device->vTable = NULL;
-            dshotMotorCount = 0;
             return false;
         }
 
         int pinIndex = DEFIO_TAG_PIN(motorConfig->ioTags[motorIndex]);
 
+        // TODO: take account of motor reordering,
+        // cf. versions of  pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t reorderedMotorIndex, motorProtocolTypes_e pwmProtocolType, uint8_t output)
         dshotMotors[motorIndex].pinIndex = pinIndex;
         dshotMotors[motorIndex].io = io;
         dshotMotors[motorIndex].pio = pio;
         dshotMotors[motorIndex].pio_sm = pio_sm;
+        dshotMotors[motorIndex].offset = offset;
 
-        uint8_t iocfg = 0;
         IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
-        IOConfigGPIO(io, iocfg);
+        // uint8_t iocfg = 0;
+        // IOConfigGPIO(io, iocfg); // TODO: don't need this? assigned and dir in program init
+        
+#ifdef USE_DSHOT_TELEMETRY
+        dshot_program_bidir_init(pio, pio_sm, dshotMotors[motorIndex].offset, pinIndex);
+#else
+        dshot_program_init(pio, pio_sm, dshotMotors[motorIndex].offset, pinIndex);
+#endif
 
-        int offset = pio_add_program(pio, &dshot_bidir_program);
+        // TODO pio_sm_set_enabled for pio_sm of dshotMotors[0..3] maybe
-        if (offset == -1) {
+        // better in vTable functions dshotEnable/DisableMotors
-            /* error loading PIO */
-            pio_sm_unclaim(pio, pio_sm);
-            device->vTable = NULL;
-            dshotMotorCount = 0;
-            return false;
-        } else {
-            dshotMotors[motorIndex].offset = offset;
-        }
-
-        dshot_bidir_program_init(pio, pio_sm, dshotMotors[motorIndex].offset, pinIndex);
         pio_sm_set_enabled(pio, pio_sm, true);
+
+        dshotMotors[motorIndex].configured = true;
+
     }
+
+    device->vTable = &dshotVTable;
+    dshotMotorCount = motorCountProvisional;
     return true;
 }