DO NOT MERGE WIP. Add support for external SPI ADC on rp2040 and rp2350

src/current/rp2350/README.md

@@ -4,6 +4,124 @@
 *warning* Work in progress
 
 Implementation of current sensing on RP2350 using the PIO to read SPI based ADCs.
+Wiring:
 
+* SCK_PIN -> Clock pin shared with 3 ADCs
+* CSB_PIN -> Chip Select pin shared with 3 ADCs
+* D0, D1 D2 -> The 3 data output from the ADCs. Needs to be contigus (D1=D0+1 D2=D1+1)
 
+Temporarly, for this to work, we need to add a PWM chanell in Arduino-FOC rp2040 hardware specific files. The duty cyctle needs to be computed to trigger the ADC at the right-time.
+
+
+TODO:
+* Find a way to setup the trigger PWM from the driver, or change the arduino-foc lib.
+* Select an available PIO and state machine automaticaly.
+
+
+# Example:
+```
+// Open loop motor control example With current readings (work in progress)
+#include <SimpleFOC.h>
+#include "pico/stdlib.h"
+#include "hardware/pio.h"
+#include "hardware/clocks.h"
+#include <SimpleFOCDrivers.h>
+#include "current/rp2350/RP2350PIOCurrentSense.h"
+
+static const uint SCK_PIN   = 2;  // sideset pin
+static const uint CSB_PIN   = 3;  // set pin
+static const uint D0_PIN    = 4;  // D0..D2 must be contiguous (D1=D0+1, D2=D0+2)
+static const uint TRIG_PIN  = 8;  // This need to be also configure as an extra PWM output with custom fixed duty cycle. For now let's just one of the phase as the trigger.
+
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(7, 8, 9);
+RP2350PIOCurrentSense curr = RP2350PIOCurrentSense(1.0, 4096,SCK_PIN, CSB_PIN, D0_PIN, TRIG_PIN);
+
+float target_velocity = 0.2;
+
+void setup() {
+  Serial.begin(115200);
+  SimpleFOCDebug::enable(&Serial);
+  driver.voltage_power_supply = 12;
+  driver.voltage_limit = 12;
+  driver.init();
+  motor.linkDriver(&driver);
+  motor.voltage_limit = 2;   // [V]
+  motor.controller = MotionControlType::velocity_openloop;
+  motor.init();
+  Serial.println("Motor ready!");
+  _delay(100);
+  curr.init();
+}
+
+void loop() {
+  motor.move(target_velocity);
+  PhaseCurrent_s phaseCurrents = curr.getPhaseCurrents();
+  Serial.printf("%d, %3.3lf, %3.3lf, %3.3lf\r\n", micros(), phaseCurrents.a, phaseCurrents.b, phaseCurrents.c);
+}
+```
+
+
+## Compilling the PIO program
+To manually compile the PIO files (generating a .pio.h from a pio), you need to instal pioasm.
+Note for self, on my setup:
+
+```
+export PIOASM="$HOME/.arduino15/packages/rp2040/tools/pqt-pioasm/4.1.0-1aec55e/pioasm"
+"$PIOASM" -v 1 bu79100g_parallel3.pio bu79100g_parallel3.pio.h
+```
+
+# Internal Design
+
+This library uses:
+- One PIO state machine  
+- Two DMA channels  
+- One PWM (and associated GPIO)
+
+
+## Trigger Pin
+
+For low-side current measurement, the ADC must sample at the center of the low pulse of the phase PWM signals.  
+To achieve this, a *trigger pin* is configured as an auxiliary PWM output, synchronized with the motor PWM but with a fixed duty cycle.  
+
+- This PWM can be mapped to any pin.  
+- It does **not** need to be connected to external hardware.  
+- The duty cycle is tuned to compensate for the dummy sampling enforced by the BU79100G-LA ADC.  
+
+In the future, this trigger mechanism could also be used to synchronize other types of ADCs.  
+
+**Note (TODO/WIP):** Trigger pin handling is not yet implemented in the driver. Proper synchronization during driver initialization still needs to be added.  
+
+
+## PIO
+
+The PIO waits for a rising edge on the trigger pin to start two SPI transfers:  
+1. A **dummy conversion** (required by the BU79100G-LA when operating at low frequency).  
+2. The **actual ADC conversion**.  
+
+Data is captured from 3 SPI data lines in parallel. A 4th line is also sampled but discarded; it can be freely repurposed as a normal GPIO (input or output).  
+
+The PIO writes results to its FIFO as two interleaved 32-bit words in the format: [a1 b1 c1 d1 a2 b2 c2 d2 .... ]
+
+## DMA
+
+Two DMA channels are configured to move data from the PIO FIFO into system memory:  
+
+- **dma_a** transfers incoming 32-bit words into a ring buffer.  
+  - The buffer size must be a power of two.  
+  - The buffer address must be aligned with its total size.  
+
+- **dma_b** is chained to **dma_a**. It resets the transfer count of **dma_a** by performing a single 32-bit transfer.  
+- **dma_a** is then chained back to **dma_b**, creating a self-sustaining loop without CPU intervention.  
+
+
+## CPU Access to Samples
+
+The CPU can asynchronously access the most recent samples by reading the current DMA write address.  
+
+- If the address is **even**, read samples at positions *n-2* and *n-1*.  
+- If the address is **odd**, read samples at positions *n-3* and *n-2*.  
+- Address wraparound is handled.
+
+Since the PIO delivers interleaved data, the CPU must de-interleave it. On an RP2350, de-interleaving and copying takes approximately **1 µs** per sample set.  
 

src/current/rp2350/RP2350PIOCurrentSense.cpp

@@ -1,9 +1,13 @@
     #include "RP2350PIOCurrentSense.h"
-
-    RP2350PIOCurrentSense::RP2350PIOCurrentSense(float gain, uint32_t max_adc_value, int pinA, int pinB, int pinC) : CurrentSense() {
-        this->pinA = pinA;
-        this->pinB = pinB;
-        this->pinC = pinC;
+    #if defined(ARDUINO_ARCH_RP2040)
+    RP2350PIOCurrentSense::RP2350PIOCurrentSense(PIO pio, float gain, uint32_t max_adc_value, int pinSCK, int pinCSB, int pinD0, int pinTRIG) : CurrentSense() {
+        this->pio = pio;
+        this->pinSCK = pinSCK;
+        this->pinCSB = pinCSB;
+        this->pinD0 = pinD0;
+        this->pinD1 = pinD0+1;
+        this->pinD2 = pinD0+2;
+        this->pinTRIG = pinTRIG;
         this->gain_a = gain;
         this->gain_b = gain;
         this->gain_c = gain;
@@ -19,22 +23,138 @@
         // TODO check that pins are valid for PIO use (e.g. consecutive pins on same bank)
         // TODO check that driver is linked
 
-        // TODO init PIO        
-        // TODO init ADC via SPI
-        // TODO init DMA to transfer ADC data to memory buffer
+        // Done init PIO        
+        // TODO init ADC via SPI (Only for ADC with MOSI input)
+        // Done init DMA to transfer ADC data to memory buffer
         // TODO init timer to trigger PIO conversions at required frequency (check driver settings)
         //      TDB: do we need config input to know which timer slice and channel to use? or can we pick automatically?
         // TODO start everything up
+        // TODO implement offset and calibration like other current sensor classes.
+        float sck_hz = 20e6;
+        int sm = pio_claim_unused_sm(this->pio, true);
+        //if (sm < 0) { pio = pio1; sm = pio_claim_unused_sm(pio1, true); } //For now, let say we have to use PIO0, this is simpler for quick DMA setup
+
+        // --- patch program instructions with chosen trigger pin ---
+        size_t prog_len = bu79100g_parallel3_program.length;
+        uint16_t insns[prog_len];
+        memcpy(insns, bu79100g_parallel3_program_instructions, sizeof(insns));
+        insns[1] = (insns[1] & ~0x1Fu) | (this->pinTRIG & 0x1Fu);
+        insns[2] = (insns[2] & ~0x1Fu) | (this->pinTRIG & 0x1Fu);
+        struct pio_program prog = bu79100g_parallel3_program; // copy metadata
+        prog.instructions = insns;
+
+        uint off = pio_add_program(this->pio, &prog);
+        pio_sm_config c = bu79100g_parallel3_program_get_default_config(off);
+
+        // Map pins to the SM
+        sm_config_set_in_pins(&c, this->pinD0);                // reads D0..D2
+        sm_config_set_set_pins(&c, this->pinCSB, 1);           // CSB (1 pin)
+        sm_config_set_sideset_pins(&c, this->pinSCK);          // SCK (sideset)
+
+        // Put pins into PIO control
+        pio_gpio_init(this->pio, this->pinSCK);
+        pio_gpio_init(this->pio, this->pinCSB);
+        pio_gpio_init(this->pio, this->pinD0);
+        pio_gpio_init(this->pio, this->pinD1);
+        pio_gpio_init(this->pio, this->pinD2);
+
+        // Directions (from the SM’s point of view)
+        pio_sm_set_consecutive_pindirs(pio, sm, this->pinSCK, 1, true);   // SCK out
+        pio_sm_set_consecutive_pindirs(pio, sm, this->pinCSB, 1, true);   // CS out
+        pio_sm_set_consecutive_pindirs(pio, sm, this->pinD0,  3, false);  // D0..D2 in
+
+        // Shift config: right, autopush every 32 bits (two pushes per conversion)
+        sm_config_set_in_shift(&c, false, true, 32);
+
+        // SCK ≈ clk_sys / (2 * clkdiv) because each SCK period = 2 instructions
+        float div = (float)clock_get_hz(clk_sys) / (2.0f * sck_hz);
+        sm_config_set_clkdiv(&c, div);
+
+        // Init the SM
+        pio_sm_init(pio, sm, off, &c);
+
+       //DMA Setup
+       //dma_a is set to read from PIO RX FIFO and write to 'buff' buffer memory
+        dma_a = dma_claim_unused_channel(true);
+        dma_b = dma_claim_unused_channel(true);
+        // ---------------------- DMA A: PIO RX FIFO -> buff[] with WRITE ring ----------------------
+        dma_channel_config ca = dma_channel_get_default_config(dma_a);
+        channel_config_set_read_increment(&ca, false);
+        channel_config_set_write_increment(&ca, true);
+        channel_config_set_transfer_data_size(&ca, DMA_SIZE_32);
+        channel_config_set_dreq(&ca, DREQ_PIO0_RX0 + sm);
+        channel_config_set_chain_to(&ca, dma_b);  // A -> B when count hits 0
+
+        // Enable WRITE-side ring over the whole buffer span (power-of-two bytes)
+        // size_bits = log2(RING_BYTES)
+        channel_config_set_ring(&ca, /*write=*/true, /*size_bits=*/__builtin_ctz(RING_BYTES));
+
+        dma_channel_set_config(dma_a, &ca, false);
+        dma_channel_set_read_addr(dma_a, &pio->rxf[sm], false);
+        dma_channel_set_write_addr(dma_a, (void*)buff, false);
+        dma_channel_set_trans_count(dma_a, RING_WORDS, false);
+
+        // ---------------------- DMA B: rearm A with ONE 32-bit write ----------------------
+        dma_channel_config cb = dma_channel_get_default_config(dma_b);
+        channel_config_set_read_increment(&cb, false);
+        channel_config_set_write_increment(&cb, false);
+        channel_config_set_transfer_data_size(&cb, DMA_SIZE_32);
+
+        // Write 1 word to A.AL1_TRANSFER_COUNT_TRIG (this sets count and re-starts A)
+        dma_channel_configure(
+            dma_b, &cb,
+            (void*)&dma_hw->ch[dma_a].al1_transfer_count_trig,
+            (const void*)&reload_count,
+            1, false
+        );
+
+        // Go!
+        dma_channel_start(dma_a);
+        pio_sm_set_enabled(pio, sm, true);
 
         return 0;
     };
 
     PhaseCurrent_s RP2350PIOCurrentSense::getPhaseCurrents() {
         PhaseCurrent_s current;
-        // TODO copy values from latest ADC reading
-        // TODO process raw values to get currents in mAmps
+
+        const uintptr_t base = (uintptr_t)buff;
+        //Get the index the DMA is about to write
+        const uint32_t i_dma = (dma_hw->ch[dma_a].write_addr - base)>>2;
+        //For a safe read, get the one that is an even number and at least <2.wi, manage looping
+        const uint32_t i_last = (i_dma <= 1) ? RING_WORDS -2 : ((i_dma / 2)*2 - 2);
+        //copy them quickly (before any print!)
+        const uint32_t w0 = buff[i_last];
+        const uint32_t w1 = buff[i_last+1];
+
+        //Reconstruct the 3 current from interleaved data
+        uint32_t a,b,c = 0;
+        uint32_t g;
+
+        //g = (w0 >> 28) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u); d = (d<<1)|((g>>3)&1u);
+        //g = (w0 >> 24) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u); d = (d<<1)|((g>>3)&1u);
+        //g = (w0 >> 20) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u); d = (d<<1)|((g>>3)&1u);
+        //g = (w0 >> 16) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u); d = (d<<1)|((g>>3)&1u);
+        g = (w0 >> 12) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w0 >>  8) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w0 >>  4) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w0 >>  0) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+
+        g = (w1 >> 28) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w1 >> 24) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w1 >> 20) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w1 >> 16) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w1 >> 12) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w1 >>  8) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w1 >>  4) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+        g = (w1 >>  0) & 0xFu; a = (a<<1)|((g>>0)&1u); b = (b<<1)|((g>>1)&1u); c = (c<<1)|((g>>2)&1u);// d = (d<<1)|((g>>3)&1u);
+
+        current.a = a * gain_a;
+        current.b = b * gain_b;
+        current.c = c * gain_c;
+
         return current;
     };
-
+    #endif
 
 

src/current/rp2350/RP2350PIOCurrentSense.h

@@ -1,17 +1,42 @@
 
 #pragma once
-
+#if defined(ARDUINO_ARCH_RP2040)
 #include "common/base_classes/CurrentSense.h"
-
+#include "bu79100g_parallel3.pio.h"
+#include "hardware/dma.h"
+#include "hardware/sync.h"
 
 class RP2350PIOCurrentSense: public CurrentSense {
   public:
-    RP2350PIOCurrentSense(float gain, uint32_t max_adc_value, int pinA, int pinB, int pinC = _NC);
+    RP2350PIOCurrentSense(PIO pio, float gain, uint32_t max_adc_value, int pinSCK, int pinCSB, int pinD0, int pinTRIG);
     ~RP2350PIOCurrentSense();
 
     int init() override;
 
     PhaseCurrent_s getPhaseCurrents() override;
-  protected:
+
+    static constexpr uint32_t RING_WORDS    = 64;   // ring span (needs to be a power of two)
+    static constexpr uint32_t RING_BYTES = RING_WORDS * 4;
+
+    // Buffer base must be aligned to ring span for write-ring:
+    alignas(RING_BYTES) volatile uint32_t buff[RING_WORDS];
+
+    // Single word used by DMA B to rearm A:
+    alignas(4) volatile uint32_t reload_count = RING_WORDS;
+
+    int dma_a = -1; // PIO RX -> ring (streamer)
+    int dma_b = -1; // reloader 
+
+    PIO pio;
+
     uint32_t max_adc_value; //!< maximum ADC value (e.g. 4096 for 12 bit ADC)
+    int pinCSB;
+    int pinSCK;
+    int pinD0;
+    int pinD1;
+    int pinD2;
+    int pinTRIG;
+
+  protected: //For debug, all public
 };
+#endif

src/current/rp2350/bu79100g_parallel3.pio

@@ -0,0 +1,34 @@
+.program bu79100g_parallel3
+; BU79100G-LA parallel read of 3 ADCs with a trigger GPIO
+; sideset pin: SCK (1 pin), CPOL=1 idle HIGH
+; set pin:     CSB (1 pin)
+; in_base:     D0..D2 (3 contiguous input pins)
+
+.side_set 1 opt            ; 1 sideset bit to drive SCK
+.wrap_target
+    set pins, 1   side 1   ; CSB=1, SCK=1
+    ; -------- wait external falling edge on absolute GPIO ----------
+    wait 1 gpio 11 ;THIS INSTRUCTION WILL BE PATCHED AT RUNTIME TO CHANGE GPIO. DO NOT MOVE IT WITHOUT CHANGING PIO INIT.
+    wait 0 gpio 11 ;THIS INSTRUCTION WILL BE PATCHED AT RUNTIME TO CHANGE GPIO. DO NOT MOVE IT WITHOUT CHANGING PIO INIT.
+    set pins, 0   side 1          ; CS=0, SCK=1
+    set x, 15     side 1          ; prepare loop cpt
+    ; -------- 16 dummy clocks (no sampling) ----------------------- 
+dummy_loop:
+        nop           side 0          ; SCK=0
+        jmp x--, dummy_loop side 1    ; SCK=1
+    set pins, 1   side 1       ; CSB=0, keep SCK=HIGH at CSB falling (datasheet rec.)
+    set x, 15     side 1  
+    set pins, 0
+    ; -------- 16 real clocks, sample on SCK falling edge -----------
+read_loop:    
+        in  pins, 4   side 0          ; SCK=0, sample D2..D0 on falling edge  ;Note, Here I had to read 4 pins instead of 3. It seems that with 3, a bit is lost after the first push. Need to read the doc again
+        jmp x--, read_loop side 1      ; SCK=1
+    ; -------- finish conversion -----------------------------------
+    set pins, 1   side 1               ; CSB=1, SCK=1
+    ; -------- Pad with 0 ------------------------------------------
+    ;set x, 0        ; Clear x
+    ;mov x, ~x 
+    ;in x, 16        ; Push 16 bits of 1 into ISR
+
+    ;push 
+.wrap