From af85e2eb7148b5af3daaafca035f58e5227ff1fa Mon Sep 17 00:00:00 2001 From: Christopher Wun Date: Tue, 2 Sep 2025 14:21:32 -0700 Subject: [PATCH] ringbuf + ads changes only --- lib/cionic/ringbuf.c | 162 ++++++++++++++++++++++++++++++ lib/cionic/ringbuf.h | 45 +++++++++ ports/nordic/Makefile | 1 + shared-bindings/ads1x9x/ADS1x9x.c | 22 ++-- shared-module/ads1x9x/ADS1x9x.c | 106 ++++++++++++++----- shared-module/ads1x9x/ADS1x9x.h | 10 +- 6 files changed, 311 insertions(+), 35 deletions(-) create mode 100644 lib/cionic/ringbuf.c create mode 100644 lib/cionic/ringbuf.h diff --git a/lib/cionic/ringbuf.c b/lib/cionic/ringbuf.c new file mode 100644 index 0000000000000..ecfeda49d537d --- /dev/null +++ b/lib/cionic/ringbuf.c @@ -0,0 +1,162 @@ +#include "shared-bindings/util.h" +#include "py/gc.h" +#include +#include +#include +#include +#include "shared/runtime/interrupt_char.h" + +#include "ringbuf.h" + +ringbuf_t *cionic_ringbuf_alloc(uint16_t sample_size, uint16_t nsamples) +{ + int buflen = nsamples*sample_size; + ringbuf_t *rb = NULL; + + rb = gc_alloc((sizeof(ringbuf_t) + buflen), false); + // ** removed the third param 'long_lived'=true from gc_alloc call (was included in our v7 code) because it is not supported in CP v9 anymore + // 'false' flag is for alloc_flags instead (consistent with v7 code) + + if(rb == NULL){ + return NULL; + } + + rb->cbuflen = buflen; + rb->sample_size = sample_size; + rb->write_idx = rb->read_idx = 0; + atomic_flag_clear(&rb->lock); + + return rb; +} + +void cionic_ringbuf_lock(ringbuf_t *rb) { + while (atomic_flag_test_and_set(&rb->lock)) { + // spinning... no OS or threading support for full mutex in circuitpython + mp_handle_pending(true); + // Allow user to break out of a timeout with a KeyboardInterrupt. + if (mp_hal_is_interrupted()) { + return; + } + } +} + +void cionic_ringbuf_unlock(ringbuf_t *rb) { + atomic_flag_clear(&rb->lock); +} + +void cionic_ringbuf_clear(ringbuf_t *rb) +{ + cionic_ringbuf_lock(rb); + rb->write_idx = rb->read_idx = 0; + cionic_ringbuf_unlock(rb); +} + +int cionic_ringbuf_len(ringbuf_t *rb) +{ + int len; + cionic_ringbuf_lock(rb); + len = (rb->write_idx + rb->cbuflen - rb->read_idx) % rb->cbuflen; + cionic_ringbuf_unlock(rb); + return len; +} + +// locked read +// fill buf with as many whole samples as will fit; return number of bytes written into buf +int cionic_ringbuf_read_samples(ringbuf_t *rb, void *_buf, int buflen) +{ + int ret; + cionic_ringbuf_lock(rb); + ret = cionic_ringbuf_read_samples_nolock(rb, _buf, buflen); + cionic_ringbuf_unlock(rb); + return ret; +} + +// locked write +// write all buflen bytes from buf into ringbuf, or return false +bool cionic_ringbuf_write_sample(ringbuf_t *rb, const void *_buf, int buflen) +{ + bool ret; + cionic_ringbuf_lock(rb); + ret = cionic_ringbuf_write_sample_nolock(rb, _buf, buflen); + cionic_ringbuf_unlock(rb); + return ret; +} + +// nolock read +// fill buf with as many whole samples as will fit; return number of bytes written into buf +int cionic_ringbuf_read_samples_nolock(ringbuf_t *rb, void *_buf, int buflen) +{ + uint8_t *buf = _buf; + const int nsamples = buflen / rb->sample_size; + const int copylen = nsamples * rb->sample_size; + + // bytes in buffer waiting to be read + int backavail = 0; // nbytes from read_idx + int frontavail = 0; // nbytes from 0 to write_idx + + if (rb->write_idx >= rb->read_idx) { + // from read_idx to write_idx + backavail = rb->write_idx - rb->read_idx; + frontavail = 0; + } else { + // from read_idx to end of buf, and then from 0 to write_idx + backavail = rb->cbuflen - rb->read_idx; + frontavail = rb->write_idx; + } + + int backlen = MIN(backavail, copylen); + memcpy(buf, &rb->cbuf[rb->read_idx], backlen); + + int frontlen = MIN(frontavail, copylen-backlen); + if (frontlen > 0) { + memcpy(buf+backlen, &rb->cbuf[0], frontlen); + } else { + frontlen = 0; + } + + rb->read_idx = (rb->read_idx + frontlen + backlen) % rb->cbuflen; + + return backlen + frontlen; +} + +// nolock write +// write all buflen bytes from buf into ringbuf, or return false +bool cionic_ringbuf_write_sample_nolock(ringbuf_t *rb, const void *_buf, int buflen) +{ + const uint8_t *buf = _buf; + + // space available in rb + int backavail = 0; // nbytes from write_idx + int frontavail = 0; // nbytes from 0 + if (rb->write_idx >= rb->read_idx) { + // from write_idx to end of buf and then from 0 to read_idx-1 + backavail = rb->cbuflen - rb->write_idx; + frontavail = rb->read_idx - 1; + } else { + // from write_idx to read_idx-1 + backavail = rb->read_idx - rb->write_idx - 1; + frontavail = 0; + } + + if (backavail + frontavail < rb->sample_size) { + // not enough space + return false; + } + + int write_idx = rb->write_idx; + + int backlen = MIN(backavail, rb->sample_size); + memcpy(&rb->cbuf[write_idx], buf, backlen); + + int frontlen = MIN(frontavail, rb->sample_size - backlen); + if (frontlen > 0) { + memcpy(&rb->cbuf[0], buf+backlen, frontlen); + } + + // set write_idx last for thread immunity + // consider lock around read and writes for greater protection + rb->write_idx = (rb->write_idx + rb->sample_size) % rb->cbuflen; + + return true; +} + diff --git a/lib/cionic/ringbuf.h b/lib/cionic/ringbuf.h new file mode 100644 index 0000000000000..1d93a16aae166 --- /dev/null +++ b/lib/cionic/ringbuf.h @@ -0,0 +1,45 @@ +#ifndef CIONIC_RINGBUF_H_ +#define CIONIC_RINGBUF_H_ + +#include + +typedef struct ringbuf_t { + uint16_t cbuflen; + uint16_t sample_size; + uint16_t write_idx; + uint16_t read_idx; + atomic_flag lock; // use atomic for lock + uint8_t cbuf[]; +} ringbuf_t; + +// allocate new ringbuf +ringbuf_t *cionic_ringbuf_alloc(uint16_t sample_size, uint16_t nsamples); + +// lock/unlock ringbuf +void cionic_ringbuf_lock(ringbuf_t *rb); +void cionic_ringbuf_unlock(ringbuf_t *rb); + +// clear any existing data in ringbuf +void cionic_ringbuf_clear(ringbuf_t *rb); + +// return nbytes in ringbuf available to be read +int cionic_ringbuf_len(ringbuf_t *rb); + +// locked read; fill buf with as many whole samples as will fit; return number of bytes written into buf +int cionic_ringbuf_read_samples(ringbuf_t *rb, void *buf, int buflen); + +// locked write; write all buflen bytes from buf into ringbuf, or return false +bool cionic_ringbuf_write_sample(ringbuf_t *rb, const void *buf, int buflen); + +// nolock read; fill buf with as many whole samples as will fit; return number of bytes written into buf +int cionic_ringbuf_read_samples_nolock(ringbuf_t *rb, void *buf, int buflen); + +// nolock write; write all buflen bytes from buf into ringbuf, or return false +bool cionic_ringbuf_write_sample_nolock(ringbuf_t *rb, const void *buf, int buflen); + +// return number of samples in buffer +static inline int cionic_ringbuf_num_samples(ringbuf_t *rb) { + return cionic_ringbuf_len(rb)/rb->sample_size; +} + +#endif diff --git a/ports/nordic/Makefile b/ports/nordic/Makefile index 81b664bc48aff..e745f4e9cce3a 100755 --- a/ports/nordic/Makefile +++ b/ports/nordic/Makefile @@ -35,6 +35,7 @@ ifeq ($(CIONIC_LIB), 1) INC += -I../../lib/cionic SRC_C += ../../lib/cionic/diff_filter.c SRC_C += ../../lib/cionic/emg_iir.c +SRC_C += ../../lib/cionic/ringbuf.c SRC_C += ../../lib/cionic/orientation.c endif diff --git a/shared-bindings/ads1x9x/ADS1x9x.c b/shared-bindings/ads1x9x/ADS1x9x.c index 2f2de4cc42d74..69a54313ec96e 100644 --- a/shared-bindings/ads1x9x/ADS1x9x.c +++ b/shared-bindings/ads1x9x/ADS1x9x.c @@ -40,7 +40,8 @@ //| class ADS1x9x: //| """ADS1x9x Interface //| -//| Interacts with an ADS1x9x over SPI.""" +//| Interacts with an ADS1x9x over SPI. +//| """ //| //| def __init__( //| self, @@ -51,7 +52,8 @@ //| start: microcontroller.Pin, //| pwdn: microcontroller.Pin, //| ) -> None: -//| """Construct an SPI ADS1x9x object with the given properties +//| """ +//| Construct an SPI ADS1x9x object with the given properties. //| //| :param busio.SPI spi: The SPI bus //| :param microcontroller.Pin cs: The SPI chip select @@ -102,7 +104,7 @@ STATIC mp_obj_t ads1x9x_ads1x9x_make_new(const mp_obj_type_t *type, size_t n_arg } //| def reset(self) -> None: -//| """Reset the ADS1x9x +//| """Reset the ADS1x9x. //| //| :return: None""" STATIC mp_obj_t ads1x9x_ads1x9x_reset(mp_obj_t self_in) { @@ -113,7 +115,7 @@ STATIC mp_obj_t ads1x9x_ads1x9x_reset(mp_obj_t self_in) { MP_DEFINE_CONST_FUN_OBJ_1(ads1x9x_ads1x9x_reset_obj, ads1x9x_ads1x9x_reset); //| def sample_size_get(self) -> None: -//| """Get the ADS1x9x sample size +//| """Get the ADS1x9x sample size. //| //| :return: Sample size""" STATIC mp_obj_t ads1x9x_ads1x9x_sample_size_get(mp_obj_t self_in) { @@ -123,7 +125,7 @@ STATIC mp_obj_t ads1x9x_ads1x9x_sample_size_get(mp_obj_t self_in) { MP_DEFINE_CONST_FUN_OBJ_1(ads1x9x_ads1x9x_sample_size_get_obj, ads1x9x_ads1x9x_sample_size_get); //| def filter_set(self, filter) -> None: -//| """Set filter type for ADS1x9x +//| """Set filter type for ADS1x9x. //| //| :param int filter: The filter enum to write //| :return: None""" @@ -137,7 +139,7 @@ STATIC mp_obj_t ads1x9x_ads1x9x_filter_set(mp_obj_t self_in, mp_obj_t filter) { MP_DEFINE_CONST_FUN_OBJ_2(ads1x9x_ads1x9x_filter_set_obj, ads1x9x_ads1x9x_filter_set); //| def read_reg(self, address) -> int: -//| """Read a ADS1x9x register +//| """Read a ADS1x9x register. //| //| :param int address: The register address to read from //| :return: register value""" @@ -150,7 +152,7 @@ STATIC mp_obj_t ads1x9x_ads1x9x_read_reg(mp_obj_t self_in, mp_obj_t reg_addr) { MP_DEFINE_CONST_FUN_OBJ_2(ads1x9x_ads1x9x_read_reg_obj, ads1x9x_ads1x9x_read_reg); //| def write_reg(self, address, value) -> None: -//| """Write value to a ADS1x9x register +//| """Write value to a ADS1x9x register. //| //| :param int address: The register address to write to //| :param int value: The value address to write @@ -168,7 +170,7 @@ MP_DEFINE_CONST_FUN_OBJ_3(ads1x9x_ads1x9x_write_reg_obj, ads1x9x_ads1x9x_write_r //| def start(self) -> None: -//| """Start ADS1x9x sampling +//| """Start ADS1x9x sampling. //| //| :return: None""" @@ -182,7 +184,7 @@ STATIC mp_obj_t ads1x9x_ads1x9x_start(mp_obj_t self_in) { MP_DEFINE_CONST_FUN_OBJ_1(ads1x9x_ads1x9x_start_obj, ads1x9x_ads1x9x_start); //| def stop(self) -> None: -//| """Stop ADS1x9x sampling +//| """Stop ADS1x9x sampling. //| //| :return: None""" @@ -196,7 +198,7 @@ STATIC mp_obj_t ads1x9x_ads1x9x_stop(mp_obj_t self_in) { MP_DEFINE_CONST_FUN_OBJ_1(ads1x9x_ads1x9x_stop_obj, ads1x9x_ads1x9x_stop); //| def read(self, buffer) -> int: -//| """Read ADS1x9x data +//| """Read ADS1x9x data. //| //| :param buffer: Buffer to write data to //| :return: size read""" diff --git a/shared-module/ads1x9x/ADS1x9x.c b/shared-module/ads1x9x/ADS1x9x.c index d1bfa1de67e1e..e6ebf1f10d304 100644 --- a/shared-module/ads1x9x/ADS1x9x.c +++ b/shared-module/ads1x9x/ADS1x9x.c @@ -33,16 +33,25 @@ #include "shared-bindings/time/__init__.h" #include "shared-bindings/util.h" #include "shared-bindings/digitalio/DigitalInOut.h" +#include "supervisor/background_callback.h" #include "py/mperrno.h" #include #include "py/stream.h" +#include "ringbuf.h" #define ADS1x9x_BAUDRATE (8000000) +#define ADS1x9x_RINGBUF_SIZE (100) // TS (4 Bytes) + (nb_chan * sizeof(float)) + ADS1X9X_SIZE_STATUS_REG + spi_cmd (1 Byte) -#define MAX_BUF_LEN (ADS1X9X_NUM_CHAN * sizeof(float)) + ADS1X9X_SIZE_STATUS_REG + 1 +#define TS_LEN (sizeof(uint32_t)) +#define MAX_BUF_LEN ((ADS1X9X_NUM_CHAN * sizeof(float)) + ADS1X9X_SIZE_STATUS_REG + 1 + TS_LEN) -float g_ads_buffer[ADS1X9X_NUM_CHAN] = {0}; +typedef struct ads_sample_t { + uint32_t ts; + float data[ADS1X9X_NUM_CHAN]; +}ads_sample_t; + +static ads_sample_t g_ads_sample; // ads129x datasheet 9.4.1.3.3 // ads121x datasheet p.25 - Data Format @@ -127,16 +136,65 @@ STATIC void ads129x_iir_filtered(ads1x9x_ADS1x9x_obj_t *self, uint8_t *in, float } -STATIC void data_ready_cb(void *arg) { - ads1x9x_ADS1x9x_obj_t *self = (ads1x9x_ADS1x9x_obj_t *)arg; - self->lock = true; +// Helper: clock out exactly one RDATAC frame (no filtering here) +STATIC inline void ads1x9x_read_frame(ads1x9x_ADS1x9x_obj_t *self, uint8_t *dst) { + static uint8_t zeros[MAX_BUF_LEN]; // BSS -> already zeroed + common_hal_digitalio_digitalinout_set_value(&self->cs, false); + common_hal_busio_spi_transfer(self->bus, zeros, dst, self->frame_len); + common_hal_digitalio_digitalinout_set_value(&self->cs, true); +} - if (self->started == false) { - return; +// Convert one raw frame to floats per current filter setting +STATIC void ads1x9x_convert_frame(ads1x9x_ADS1x9x_obj_t *self, + const uint8_t *raw_frame, float *out) { + const uint8_t *payload = raw_frame + ADS1X9X_SIZE_STATUS_REG; + uint16_t payload_len = self->frame_len - ADS1X9X_SIZE_STATUS_REG; + + if (self->filter == ADS1x9x_RAW) { + ads129x_raw(self, (uint8_t *)payload, out); + } else if (self->filter == ADS1x9x_DIFF_FILTER) { + ads129x_diff_filtered(self, (uint8_t *)payload, out, payload_len); + } else { // ADS1x9x_IIR_FILTER + ads129x_iir_filtered(self, (uint8_t *)payload, out, payload_len); + } +} + +#ifndef ADS_BG_MAX_BURST +#define ADS_BG_MAX_BURST 8 +#endif + +STATIC void ads1x9x_bg_worker(void *data) { + ads1x9x_ADS1x9x_obj_t *self = (ads1x9x_ADS1x9x_obj_t *)data; + static uint8_t rx_buf[MAX_BUF_LEN]; // avoid stack churn + + uint16_t serviced = 0; + while (self->drdy_pending && serviced < ADS_BG_MAX_BURST) { + ads_sample_t sample; + sample.ts = (uint32_t)(common_hal_time_monotonic_ns() / 100000); // 10 µs ticks + ads1x9x_read_frame(self, rx_buf); + ads1x9x_convert_frame(self, rx_buf, sample.data); + if (!cionic_ringbuf_write_sample(self->rb, &sample, sizeof(ads_sample_t))) { + self->drops += self->drdy_pending; + self->drdy_pending = 0; + break; + } + self->drdy_pending--; + serviced++; + } + + // If more DRDY events piled up, requeue ourselves for another slice. + if (self->drdy_pending) { + background_callback_add(&self->bg_cb, ads1x9x_bg_worker, self); } +} - common_hal_ads1x9x_ADS1x9x_read_data(self, (uint8_t *)g_ads_buffer, (self->num_chan * self->sample_bytes) + ADS1X9X_SIZE_STATUS_REG); - self->lock = false; +STATIC void data_ready_cb(void *arg) { + ads1x9x_ADS1x9x_obj_t *self = (ads1x9x_ADS1x9x_obj_t *)arg; + if (!self->started) return; + self->drdy_pending++; + if (!background_callback_pending()) { + background_callback_add(&self->bg_cb, ads1x9x_bg_worker, self); + } } STATIC void ads1x9x_set_norms(ads1x9x_ADS1x9x_obj_t *self) { @@ -188,6 +246,16 @@ void common_hal_ads1x9x_ADS1x9x_construct(ads1x9x_ADS1x9x_obj_t *self, busio_spi ads1x9x_set_norms(self); diff_filter_init(&self->diff_filter); iir_filter_init(&self->iir_filter); + memset(&g_ads_sample, 0, sizeof(ads_sample_t)); + self->rb = cionic_ringbuf_alloc(sizeof(ads_sample_t), ADS1x9x_RINGBUF_SIZE); + + + if (self->rb == NULL) { + mp_raise_OSError(ENOMEM); + return; + } + + cionic_ringbuf_clear(self->rb); } void common_hal_ads1x9x_ADS1x9x_filter_set(ads1x9x_ADS1x9x_obj_t *self, uint8_t filt) { @@ -227,22 +295,25 @@ void common_hal_ads1x9x_ADS1x9x_deinit(ads1x9x_ADS1x9x_obj_t *self) { void common_hal_ads1x9x_ADS1x9x_start(ads1x9x_ADS1x9x_obj_t *self) { uint8_t wval = CMD_RDATAC; + self->drdy_pending = 0; + self->drops = 0; + self->frame_len = ADS1X9X_SIZE_STATUS_REG + self->num_chan * self->sample_bytes; if (self->started) { mp_raise_OSError(EAGAIN); return; } self->started = true; - lock_bus(self); + lock_bus(self); // take ownership for the acquisition run common_hal_digitalio_digitalinout_set_value(&self->cs, false); common_hal_busio_spi_write(self->bus, &wval, 1); common_hal_digitalio_digitalinout_set_value(&self->cs, true); - unlock_bus(self); common_hal_digitalio_digitalinout_set_value(&self->start, true); } void common_hal_ads1x9x_ADS1x9x_stop(ads1x9x_ADS1x9x_obj_t *self) { common_hal_digitalio_digitalinout_set_value(&self->start, false); + unlock_bus(self); // release after streaming } uint8_t common_hal_ads1x9x_ADS1x9x_read_reg(ads1x9x_ADS1x9x_obj_t *self, uint8_t addr) { @@ -301,21 +372,10 @@ void common_hal_ads1x9x_ADS1x9x_read_data(ads1x9x_ADS1x9x_obj_t *self, uint8_t * ads129x_iir_filtered(self, rx_buf, (float *)data, len - ADS1X9X_SIZE_STATUS_REG); } - unlock_bus(self); } size_t common_hal_ads1x9x_ADS1x9x_read(ads1x9x_ADS1x9x_obj_t *self, mp_buffer_info_t *buf, uint16_t buf_size) { uint8_t *ptr = buf->buf; - - while (self->lock) { - mp_handle_pending(true); - // Allow user to break out of a timeout with a KeyboardInterrupt. - if (mp_hal_is_interrupted()) { - return 0; - } - } - - memcpy(ptr, g_ads_buffer, buf_size); - return buf_size; + return cionic_ringbuf_read_samples(self->rb, ptr, buf_size); } diff --git a/shared-module/ads1x9x/ADS1x9x.h b/shared-module/ads1x9x/ADS1x9x.h index 7fcdbc7e93483..6b96b3bac9135 100644 --- a/shared-module/ads1x9x/ADS1x9x.h +++ b/shared-module/ads1x9x/ADS1x9x.h @@ -31,6 +31,8 @@ #include "lib/cionic/diff_filter.h" #include "lib/cionic/emg_iir.h" +#include "lib/cionic/ringbuf.h" +#include "supervisor/background_callback.h" #include "common-hal/busio/SPI.h" #include "common-hal/digitalio/DigitalInOut.h" @@ -58,9 +60,9 @@ typedef struct { digitalio_digitalinout_obj_t pwdn; diff_filter_t diff_filter; iir_filter_t iir_filter; - uint32_t sample_bytes; + ringbuf_t *rb; + uint16_t sample_bytes; bool started; - bool lock; uint8_t id; uint8_t num_chan; uint8_t filter; @@ -68,6 +70,10 @@ typedef struct { float *loff; float all_norms[ADS1X9X_NUM_CHAN]; // all channel norms uint8_t chan[ADS1X9X_NUM_CHAN]; + background_callback_t bg_cb; // background task handle + volatile uint16_t drdy_pending; // count of DRDY events to service + uint16_t frame_len; // status + channels*bytes (computed at start) + uint16_t drops; // overflow counter (ring full) } ads1x9x_ADS1x9x_obj_t; // System Commands