diff --git a/src/current/rp2350/README.md b/src/current/rp2350/README.md index 0f37358..2cdc6f9 100644 --- a/src/current/rp2350/README.md +++ b/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 +#include "pico/stdlib.h" +#include "hardware/pio.h" +#include "hardware/clocks.h" +#include +#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. diff --git a/src/current/rp2350/RP2350PIOCurrentSense.cpp b/src/current/rp2350/RP2350PIOCurrentSense.cpp index cddc569..8c7a7d7 100644 --- a/src/current/rp2350/RP2350PIOCurrentSense.cpp +++ b/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 diff --git a/src/current/rp2350/RP2350PIOCurrentSense.h b/src/current/rp2350/RP2350PIOCurrentSense.h index 3bcadd8..1b31eeb 100644 --- a/src/current/rp2350/RP2350PIOCurrentSense.h +++ b/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 \ No newline at end of file diff --git a/src/current/rp2350/bu79100g_parallel3.pio b/src/current/rp2350/bu79100g_parallel3.pio new file mode 100644 index 0000000..b8ca40b --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/current/rp2350/bu79100g_parallel3.pio.h b/src/current/rp2350/bu79100g_parallel3.pio.h new file mode 100644 index 0000000..4208b85 --- /dev/null +++ b/src/current/rp2350/bu79100g_parallel3.pio.h @@ -0,0 +1,55 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// ------------------ // +// bu79100g_parallel3 // +// ------------------ // + +#define bu79100g_parallel3_wrap_target 0 +#define bu79100g_parallel3_wrap 12 +#define bu79100g_parallel3_pio_version 1 + +static const uint16_t bu79100g_parallel3_program_instructions[] = { + // .wrap_target + 0xf801, // 0: set pins, 1 side 1 + 0x208b, // 1: wait 1 gpio, 11 + 0x200b, // 2: wait 0 gpio, 11 + 0xf800, // 3: set pins, 0 side 1 + 0xf82f, // 4: set x, 15 side 1 + 0xb042, // 5: nop side 0 + 0x1845, // 6: jmp x--, 5 side 1 + 0xf801, // 7: set pins, 1 side 1 + 0xf82f, // 8: set x, 15 side 1 + 0xe000, // 9: set pins, 0 + 0x5004, // 10: in pins, 4 side 0 + 0x184a, // 11: jmp x--, 10 side 1 + 0xf801, // 12: set pins, 1 side 1 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program bu79100g_parallel3_program = { + .instructions = bu79100g_parallel3_program_instructions, + .length = 13, + .origin = -1, + .pio_version = bu79100g_parallel3_pio_version, +#if PICO_PIO_VERSION > 0 + .used_gpio_ranges = 0x1 +#endif +}; + +static inline pio_sm_config bu79100g_parallel3_program_get_default_config(uint offset) { + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + bu79100g_parallel3_wrap_target, offset + bu79100g_parallel3_wrap); + sm_config_set_sideset(&c, 2, true, false); + return c; +} +#endif +