From fd8e42348ca139f12d1272073964ac3fb8754cdc Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Sun, 26 Jul 2026 14:36:13 +0200 Subject: [PATCH] Improve BMA580 FIFO handling --- src/SensorManager/BMA580/BMA580_Sensor.cpp | 68 +++++++++------------- src/SensorManager/BMA580/BMA580_Sensor.h | 2 +- src/SensorManager/BoneConduction.cpp | 10 +++- 3 files changed, 34 insertions(+), 46 deletions(-) diff --git a/src/SensorManager/BMA580/BMA580_Sensor.cpp b/src/SensorManager/BMA580/BMA580_Sensor.cpp index a3e4cc76..4511e7f4 100644 --- a/src/SensorManager/BMA580/BMA580_Sensor.cpp +++ b/src/SensorManager/BMA580/BMA580_Sensor.cpp @@ -237,7 +237,10 @@ int8_t BMA580::get_accel_and_int_settings(struct bma5_dev *device) } /*! - * @brief This internal API gets FIFO configurations. + * @brief This internal API sets FIFO configurations and verifies the readback. + * + * The BMA580 silently limits the FIFO to 512 bytes while the feature engine + * is enabled, so verify that the requested FIFO size was accepted. */ int8_t BMA580::get_fifo_conf(const struct bma5_fifo_conf *fifo_config, struct bma5_dev *device) { @@ -253,13 +256,17 @@ int8_t BMA580::get_fifo_conf(const struct bma5_fifo_conf *fifo_config, struct bm rslt = bma5_get_fifo_conf(&read_fifo_conf, device); bma5_check_rslt("bma5_get_fifo_conf", rslt); + if (read_fifo_conf.fifo_size != fifo_config->fifo_size) { + LOG_ERR("FIFO size mismatch: requested 0x%02X, readback 0x%02X " + "(feature engine likely still enabled)", + fifo_config->fifo_size, read_fifo_conf.fifo_size); + } + return rslt; } int BMA580::init(int odr, int watermark_level) { int8_t rslt; - struct bma580_int_map int_map, get_int_map; - //struct bma5_fifo_conf fifo_conf; /* Assign context parameter selection */ enum bma5_context context = BMA5_HEARABLE; @@ -277,19 +284,11 @@ int BMA580::init(int odr, int watermark_level) { bma5_check_rslt("bma580_init", rslt); LOG_DBG("Chip ID :0x%X", dev.chip_id); - /* Map generic interrupts to hardware interrupt pin of the sensor */ - rslt = bma580_get_int_map(&int_map, &dev); - bma5_check_rslt("bma580_get_int_map", rslt); - - /* Set FIFO full interrupt to INT2 */ - //int_map.fifo_full_int_map = BMA580_FIFO_FULL_INT_MAP_INT2; - int_map.fifo_wm_int_map = BMA580_FIFO_WM_INT_MAP_INT1; - - rslt = bma580_set_int_map(&int_map, &dev); - bma5_check_rslt("bma580_set_int_map", rslt); - - rslt = bma580_get_int_map(&get_int_map, &dev); - bma5_check_rslt("bma580_get_int_map", rslt); + /* The feature engine and FIFO share 1024 bytes of RAM. None of the + * feature-engine functions are used here, so disable it to make the full + * RAM available to the FIFO. INT1/INT2 are unused; the FIFO is polled. */ + rslt = bma5_set_feat_eng_conf(BMA5_FEAT_ENG_CTRL_DISABLE, &dev); + bma5_check_rslt("bma5_set_feat_eng_conf", rslt); rslt = get_accel_and_int_settings(&dev); bma5_check_rslt("get_accel_and_int_settings", rslt); @@ -298,17 +297,16 @@ int BMA580::init(int odr, int watermark_level) { rslt = bma5_get_fifo_conf(&fifo_conf, &dev); bma5_check_rslt("bma5_get_fifo_conf", rslt); - rslt = bma5_get_fifo_conf(&fifo_conf, &dev); - bma5_check_rslt("bma5_get_fifo_conf", rslt); - fifo_conf.fifo_cfg = BMA5_FIFO_CFG_ENABLE; fifo_conf.fifo_acc_x = BMA5_FIFO_ACC_X_ENABLE; fifo_conf.fifo_acc_y = BMA5_FIFO_ACC_Y_ENABLE; fifo_conf.fifo_acc_z = BMA5_FIFO_ACC_Z_ENABLE; fifo_conf.fifo_compression = BMA5_FIFO_COMPRESSION_ACC_16BIT; fifo_conf.fifo_sensor_time = BMA5_FIFO_SENSOR_TIME_OFF; - fifo_conf.fifo_size = BMA5_FIFO_SIZE_MAX_512_BYTES; - fifo_conf.fifo_stop_on_full = BMA5_ENABLE; //BMA5_ENABLE + fifo_conf.fifo_size = BMA5_FIFO_SIZE_MAX_1024_BYTES; + /* Keep sampling if servicing is delayed instead of leaving the FIFO + * permanently stopped with stale data. Oldest samples may be overwritten. */ + fifo_conf.fifo_stop_on_full = BMA5_DISABLE; rslt = get_fifo_conf(&fifo_conf, &dev); bma5_check_rslt("get_fifo_conf", rslt); @@ -336,32 +334,18 @@ int BMA580::stop() { int BMA580::read(bma5_sens_fifo_axes_data_16_bit *fifo_accel_data) { int8_t rslt = BMA5_OK; - uint8_t n_status = 1; - struct bma580_int_status_types int_status{}; fifoframe.fifo_avail_frames = 0; - int_status.int_src = BMA580_INT_STATUS_INT1; + /* Read all available data without relying on watermark timing. The sensor + * and MCU clocks can drift enough for a timer poll to precede the event. */ + rslt = bma5_read_fifo_data(&fifoframe, &fifo_conf, &dev); + bma5_check_rslt("bma5_read_fifo_data", rslt); - /* Get fifo full interrupt 2 status */ - rslt = bma580_get_int_status(&int_status, n_status, &dev); - bma5_check_rslt("bma580_get_int_status", rslt); - - if (int_status.int_status.fifo_wm_int_status & BMA5_ENABLE) + if (rslt == BMA5_OK && fifoframe.fifo_avail_len > 0) { - /* Read FIFO data */ - rslt = bma5_read_fifo_data(&fifoframe, &fifo_conf, &dev); - bma5_check_rslt("bma5_read_fifo_data", rslt); - - /* Set fifo full interrupt 2 status */ - rslt = bma580_set_int_status(&int_status, n_status, &dev); - bma5_check_rslt("bma580_get_int_status\n", rslt); - - if (rslt == BMA5_OK) - { - /* Parse the FIFO data to extract accelerometer and sensortime data from the FIFO buffer */ - (void)bma5_extract_acc_sens_time_16_bit(fifo_accel_data, &fifoframe, &fifo_conf, &dev); - } + /* Parse accelerometer data from the FIFO buffer. */ + (void)bma5_extract_acc_sens_time_16_bit(fifo_accel_data, &fifoframe, &fifo_conf, &dev); } return fifoframe.fifo_avail_frames; diff --git a/src/SensorManager/BMA580/BMA580_Sensor.h b/src/SensorManager/BMA580/BMA580_Sensor.h index 1ff93349..9d32ebff 100644 --- a/src/SensorManager/BMA580/BMA580_Sensor.h +++ b/src/SensorManager/BMA580/BMA580_Sensor.h @@ -44,7 +44,7 @@ /*! Macro definition */ /*! FIFO raw data buffer size */ -#define BMA580_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(520) +#define BMA580_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(1032) /*! Number of accel frames to be extracted from FIFO * Calculation: diff --git a/src/SensorManager/BoneConduction.cpp b/src/SensorManager/BoneConduction.cpp index 26d4f648..0e9746ed 100644 --- a/src/SensorManager/BoneConduction.cpp +++ b/src/SensorManager/BoneConduction.cpp @@ -25,6 +25,8 @@ const SampleRateSetting<10> BoneConduction::sample_rates = { bool BoneConduction::init(struct k_msgq * queue) { if (!_active) { pm_device_runtime_get(ls_1_8); + pm_device_runtime_get(ls_3_3); + k_msleep(50); _active = true; } @@ -32,6 +34,7 @@ bool BoneConduction::init(struct k_msgq * queue) { LOG_WRN("Could not find a valid bone conduction sensor, check wiring!"); _active = false; pm_device_runtime_put(ls_1_8); + pm_device_runtime_put(ls_3_3); return false; } @@ -113,14 +116,14 @@ void BoneConduction::start(int sample_rate_idx) { t_sample_us = 1000000.0f / sample_rates.true_sample_rates[sample_rate_idx]; - k_timeout_t t = K_USEC(t_sample_us); - int word_size = 3 * sizeof(int16_t) + 1; - _num_samples_buffered = MIN(MAX(1, (int) (CONFIG_SENSOR_LATENCY_MS * 1000.0f / t_sample_us)), 512 / word_size - 8); // Buffer size is 512 bytes + _num_samples_buffered = MIN(MAX(1, (int) (CONFIG_SENSOR_LATENCY_MS * 1000.0f / t_sample_us)), 1024 / word_size - 8); // Buffer size is 1024 bytes bma580.init(sample_rates.reg_vals[sample_rate_idx], _num_samples_buffered * word_size); bma580.start(); + /* Drain the FIFO once per buffered block instead of once per sample. */ + k_timeout_t t = K_USEC(t_sample_us * _num_samples_buffered); k_timer_start(&sensor.sensor_timer, K_NO_WAIT, t); _running = true; @@ -139,4 +142,5 @@ void BoneConduction::stop() { bma580.stop(); pm_device_runtime_put(ls_1_8); + pm_device_runtime_put(ls_3_3); }