From dc2b5a40afa395f58159b8ea0e3212939cb7ea8b Mon Sep 17 00:00:00 2001 From: mkuettner97 Date: Mon, 7 Sep 2026 14:42:42 +0200 Subject: [PATCH 1/2] Improve high-rate PPG acquisition --- src/SensorManager/MAXM86161/MAXM86161.cpp | 126 +++++++++++++++++++--- src/SensorManager/MAXM86161/MAXM86161.h | 26 +++-- src/SensorManager/PPG.cpp | 106 ++++++++++++++++-- 3 files changed, 229 insertions(+), 29 deletions(-) diff --git a/src/SensorManager/MAXM86161/MAXM86161.cpp b/src/SensorManager/MAXM86161/MAXM86161.cpp index 8774d01f..fd39a781 100644 --- a/src/SensorManager/MAXM86161/MAXM86161.cpp +++ b/src/SensorManager/MAXM86161/MAXM86161.cpp @@ -1,10 +1,12 @@ #include "MAXM86161.h" +#include +#include #include #include LOG_MODULE_REGISTER(MAXM86161, 3); -char databuffer[32*BYTES_PER_CH*LED_NUM]; +uint8_t databuffer[32*BYTES_PER_CH*LED_NUM]; /*****************************************************************************/ // Constructor @@ -136,19 +138,28 @@ int MAXM86161::stop(void) return status; } -int MAXM86161::read(ppg_sample * buffer) { +int MAXM86161::read(ppg_sample *buffer, size_t buffer_capacity) { + if (buffer == nullptr || buffer_capacity == 0) { + return 0; + } + int status; int number_of_bytes; - int num_samples = 0; + int fifo_items = 0; int output_idx = -1; - status = _read_from_reg(REG_FIFO_DATA_COUNTER, num_samples); + status = _read_from_reg(REG_FIFO_DATA_COUNTER, fifo_items); if (status == 0){ - number_of_bytes = num_samples / LED_NUM * LED_NUM * BYTES_PER_CH; + int items_to_read = MIN(fifo_items, (int)(buffer_capacity * _exposure_count)); + items_to_read -= items_to_read % _exposure_count; + number_of_bytes = items_to_read * BYTES_PER_CH; - (void)_read_block(REG_FIFO_DATA, number_of_bytes, (uint8_t *) databuffer); + status = _read_block(REG_FIFO_DATA, number_of_bytes, (uint8_t *) databuffer); + if (status != 0) { + return 0; + } - for (int i=0; i < num_samples / LED_NUM * LED_NUM; i++) { + for (int i = 0; i < items_to_read; i++) { int idx = BYTES_PER_CH * i; uint32_t val = databuffer[idx] << 16 | databuffer[idx + 1] << 8 | databuffer[idx+2]; @@ -158,10 +169,16 @@ int MAXM86161::read(ppg_sample * buffer) { //LOG_INF("tag: %i, val: %i", tag, val); - if (tag == 1) output_idx++; - if (tag > 6 || output_idx < 0) continue; + if (tag == 1) { + if ((size_t)(output_idx + 1) >= buffer_capacity) { + break; + } + output_idx++; + memset(buffer[output_idx], 0, sizeof(ppg_sample)); + } + if (tag == 0 || tag > _exposure_count || output_idx < 0) continue; - buffer[output_idx][tag-1] = val; + buffer[output_idx][_exposure_output_indices[tag - 1]] = val; } } @@ -176,7 +193,10 @@ int MAXM86161::set_interrogation_rate(int rate) int status; // Get value of register to avoid overwriting sample average value - _read_from_reg(REG_PPG_CONFIG2, existing_reg_values); + status = _read_from_reg(REG_PPG_CONFIG2, existing_reg_values); + if (status != 0) { + return status; + } // Set the appropriate bits, while leaving the others. existing_reg_values = _set_multiple_bits(existing_reg_values, MASK_SMP_AVE, rate, POS_PPG_SR); @@ -185,6 +205,18 @@ int MAXM86161::set_interrogation_rate(int rate) return status; } +int MAXM86161::get_interrogation_rate(int &rate) +{ + int register_value; + int status = _read_from_reg(REG_PPG_CONFIG2, register_value); + + if (status == 0) { + rate = (register_value & MASK_PPG_SR) >> POS_PPG_SR; + } + + return status; +} + int MAXM86161::set_sample_averaging(int average) { int existing_reg_values; @@ -245,7 +277,10 @@ int MAXM86161::set_ppg_tint(int time) int status; // Get value of register to avoid overwriting sample average value - _read_from_reg(REG_PPG_CONFIG1, existing_reg_values); + status = _read_from_reg(REG_PPG_CONFIG1, existing_reg_values); + if (status != 0) { + return status; + } // Set the appropriate bits, while leaving the others. existing_reg_values = _set_multiple_bits(existing_reg_values, MASK_PPG_TINT_WRITE, time, POS_PPG_TINT); @@ -255,6 +290,71 @@ int MAXM86161::set_ppg_tint(int time) } +int MAXM86161::get_ppg_tint(int &time) +{ + int register_value; + int status = _read_from_reg(REG_PPG_CONFIG1, register_value); + + if (status == 0) { + time = register_value & MASK_PPG_TINT; + } + + return status; +} + +int MAXM86161::set_exposure_count(uint8_t count) +{ + /* Keep IR as the highest-rate channel; preserve the red/IR pair at 2 kHz. */ + static const uint8_t sequence_registers[LED_NUM][3] = { + { 0x02, 0x00, 0x00 }, /* IR */ + { 0x32, 0x00, 0x00 }, /* IR, red */ + { 0x12, 0x03, 0x00 }, /* IR, green, red */ + { 0x12, 0x93, 0x00 }, /* IR, green, red, ambient */ + }; + static const uint8_t output_indices[LED_NUM][LED_NUM] = { + { 1, 0, 0, 0 }, + { 1, 0, 0, 0 }, + { 1, 2, 0, 0 }, + { 1, 2, 0, 3 }, + }; + + if (count < 1 || count > LED_NUM) { + return -EINVAL; + } + + const int registers[] = { REG_LED_SEQ1, REG_LED_SEQ2, REG_LED_SEQ3 }; + for (size_t i = 0; i < 3; ++i) { + int status = _write_to_reg(registers[i], sequence_registers[count - 1][i]); + if (status != 0) { + return status; + } + + int effective_value; + status = _read_from_reg(registers[i], effective_value); + if (status != 0) { + return status; + } + if (effective_value != sequence_registers[count - 1][i]) { + return -EIO; + } + } + + int fifo_config; + int status = _read_from_reg(REG_FIFO_CONFIG2, fifo_config); + if (status != 0) { + return status; + } + status = _write_to_reg(REG_FIFO_CONFIG2, fifo_config | 0x10); + if (status != 0) { + return status; + } + + _exposure_count = count; + memcpy(_exposure_output_indices, output_indices[count - 1], + sizeof(_exposure_output_indices)); + return 0; +} + /*******************************************************************************/ int MAXM86161::alc_on(void) { @@ -389,7 +489,7 @@ int MAXM86161::_write_to_reg(int address, int value) { _i2c->release(); - return 0; + return ret; } diff --git a/src/SensorManager/MAXM86161/MAXM86161.h b/src/SensorManager/MAXM86161/MAXM86161.h index 3037dd78..cc74bcad 100644 --- a/src/SensorManager/MAXM86161/MAXM86161.h +++ b/src/SensorManager/MAXM86161/MAXM86161.h @@ -6,6 +6,7 @@ //#include #include +#include #include @@ -34,6 +35,11 @@ typedef uint32_t ppg_sample[6]; +// Constants for reading data +#define BYTES_PER_CH 3 +#define LED_NUM 4 // 3 LEDs plus the ambient sample +#define FIFO_SIZE 128 + class MAXM86161 { public: /** @brief Constructor @@ -50,11 +56,13 @@ class MAXM86161 { /** @brief Stop collecting data samples */ int stop(void); /** @brief Read data from the sensor */ - int read(ppg_sample * buffer); + int read(ppg_sample *buffer, size_t buffer_capacity); // Configuration adjustments /** @brief Set the rate of the PPG sensor */ int set_interrogation_rate(int rate); + /** @brief Read back the effective rate selected by the sensor. */ + int get_interrogation_rate(int &rate); /** @brief Set the number of samples to average */ int set_sample_averaging(int average); /** @brief Set the LED current for all LEDs */ @@ -67,6 +75,10 @@ class MAXM86161 { int set_led3_current(int current); /** @brief Set the integration time for the photodiode */ int set_ppg_tint(int time); + /** @brief Read back the effective photodiode integration time. */ + int get_ppg_tint(int &time); + /** @brief Configure the number of optical exposures in each sample. */ + int set_exposure_count(uint8_t count); // Setting adjustments /** @brief Set the ALC on */ @@ -97,6 +109,9 @@ class MAXM86161 { uint8_t _addr = DT_REG_ADDR(DT_NODELABEL(maxm86161)); + uint8_t _exposure_count = LED_NUM; + uint8_t _exposure_output_indices[LED_NUM] = { 1, 2, 0, 3 }; + // void _set_led_sequence(char sequence); int _read_from_reg(int address, int &data); @@ -118,12 +133,6 @@ class MAXM86161 { // Part ID of the MAXM86161 #define PPG_PART_ID 0x36 -// Constants for reading data -#define BYTES_PER_CH 3 -#define LED_NUM 4 // 3 LEDs plus the ambient sample -#define FIFO_SIZE 128 - - // Bit positions. #define POS_DATA_RDY_EN 6 @@ -145,6 +154,7 @@ class MAXM86161 { #define MASK_SMP_AVE 0b00000111 //Register 0x12 #define MASK_PPG_SR 0b11111000 // Register 0x12 #define MASK_PPG_TINT_WRITE 0b11111100 // Register 0x11 +#define MASK_PPG_TINT 0b00000011 // Register 0x11 #define MASK_PPG_LABEL 0x7FFFF; #define MASK_PPG_ID 0xF8; @@ -228,4 +238,4 @@ class MAXM86161 { #define REG_FIFO_TAG_MASK 0x1F -#endif /* __MAXM86161_H_*/ \ No newline at end of file +#endif /* __MAXM86161_H_*/ diff --git a/src/SensorManager/PPG.cpp b/src/SensorManager/PPG.cpp index 1358fbec..7b4ee7e0 100644 --- a/src/SensorManager/PPG.cpp +++ b/src/SensorManager/PPG.cpp @@ -4,12 +4,11 @@ #include "math.h" #include "stdlib.h" +#include #include LOG_MODULE_DECLARE(MAXM86161); -#define LATENCY_MS 40 - PPG PPG::sensor; MAXM86161 PPG::ppg(&I2C2); @@ -27,6 +26,41 @@ const SampleRateSetting<16> PPG::sample_rates = { 32.000, 64.000, 128.000, 256.000, 512.000, 1024.000, 2048.000, 4096.000}, }; +namespace { + +struct ppg_timing { + int integration_time; + uint8_t exposure_count; +}; + +/* MAXM86161 data-sheet limits for four exposures per sample. */ +constexpr float max_rate_for_four_exposures[] = { + 1024.0f, /* 14.8 us */ + 512.0f, /* 29.4 us */ + 512.0f, /* 58.7 us */ + 400.0f, /* 117.3 us */ +}; + +ppg_timing select_timing(float requested_rate) +{ + for (int tint = 3; tint >= 0; --tint) { + if (requested_rate <= max_rate_for_four_exposures[tint]) { + return { tint, 4 }; + } + } + + if (requested_rate <= 2048.0f) { + return { 0, 2 }; + } + if (requested_rate <= 4096.0f) { + return { 0, 1 }; + } + + return { -ENOTSUP, 0 }; +} + +} // namespace + bool PPG::init(struct k_msgq * queue) { if (!_active) { pm_device_runtime_get(ls_1_8); @@ -87,7 +121,8 @@ void PPG::update_sensor(struct k_work *work) { } if(int_status & MAXM86161_INT_FULL) { // MAXM86161_INT_DATA_RDY - int num_samples = ppg.read(sensor.data_buffer); + int num_samples = ppg.read(sensor.data_buffer, + sizeof(sensor.data_buffer) / sizeof(sensor.data_buffer[0])); PPG::sensor._sample_count = MAX(0, PPG::sensor._num_samples_buffered - num_samples); @@ -138,15 +173,70 @@ void PPG::sensor_timer_handler(struct k_timer *dummy) { void PPG::start(int sample_rate_idx) { if (!_active) return; - t_sample_us = 1000000.0f / sample_rates.true_sample_rates[sample_rate_idx]; + const float requested_rate = sample_rates.true_sample_rates[sample_rate_idx]; + const int requested_rate_register = sample_rates.reg_vals[sample_rate_idx]; + const ppg_timing timing = select_timing(requested_rate); + + if (timing.integration_time < 0) { + LOG_ERR("PPG rate %.3f Hz is not supported", (double)requested_rate); + return; + } + + int ret = ppg.set_exposure_count(timing.exposure_count); + if (ret != 0) { + LOG_ERR("Failed to set PPG exposure count to %u: %d", timing.exposure_count, ret); + return; + } + + ret = ppg.set_ppg_tint(timing.integration_time); + if (ret != 0) { + LOG_ERR("Failed to set PPG integration time: %d", ret); + return; + } + + int effective_integration_time = -1; + ret = ppg.get_ppg_tint(effective_integration_time); + if (ret != 0 || effective_integration_time != timing.integration_time) { + LOG_ERR("PPG rejected integration time %d (effective %d, ret %d)", + timing.integration_time, effective_integration_time, ret); + return; + } + + ret = ppg.set_interrogation_rate(requested_rate_register); + if (ret != 0) { + LOG_ERR("Failed to set PPG sample rate: %d", ret); + return; + } + + int effective_rate_register = -1; + ret = ppg.get_interrogation_rate(effective_rate_register); + if (ret != 0 || effective_rate_register != requested_rate_register) { + LOG_ERR("PPG rejected rate register 0x%02x (effective 0x%02x, ret %d)", + requested_rate_register, effective_rate_register, ret); + return; + } + + t_sample_us = 1000000.0f / requested_rate; k_timeout_t t = K_USEC(t_sample_us); - _num_samples_buffered = MIN(MAX(1, (int) (CONFIG_SENSOR_LATENCY_MS * 1000.0f / t_sample_us)), FIFO_SIZE / LED_NUM - 2); + const int fifo_sample_capacity = FIFO_SIZE / timing.exposure_count - 2; + const int work_buffer_capacity = + sizeof(sensor.data_buffer) / sizeof(sensor.data_buffer[0]) - 2; + _num_samples_buffered = MIN(MAX(1, (int)(CONFIG_SENSOR_LATENCY_MS * 1000.0f / t_sample_us)), + MIN(fifo_sample_capacity, work_buffer_capacity)); - ppg.set_interrogation_rate(sample_rates.reg_vals[sample_rate_idx]); - ppg.set_watermark(FIFO_SIZE - _num_samples_buffered * LED_NUM); - ppg.start(); + ret = ppg.set_watermark(FIFO_SIZE - _num_samples_buffered * timing.exposure_count); + if (ret != 0) { + LOG_ERR("Failed to set PPG FIFO watermark: %d", ret); + return; + } + + ret = ppg.start(); + if (ret != 0) { + LOG_ERR("Failed to start PPG: %d", ret); + return; + } k_timer_start(&sensor.sensor_timer, K_NO_WAIT, t); From 0d24acafcd7e2a808246412bf06f5fd1c1eb739f Mon Sep 17 00:00:00 2001 From: Oliver Bagge Date: Fri, 18 Sep 2026 16:19:01 +0200 Subject: [PATCH 2/2] fix(ppg): preserve partial samples across FIFO reads --- src/SensorManager/MAXM86161/MAXM86161.cpp | 42 +++++++++++++++++------ src/SensorManager/MAXM86161/MAXM86161.h | 2 ++ 2 files changed, 33 insertions(+), 11 deletions(-) diff --git a/src/SensorManager/MAXM86161/MAXM86161.cpp b/src/SensorManager/MAXM86161/MAXM86161.cpp index fd39a781..dc9387cf 100644 --- a/src/SensorManager/MAXM86161/MAXM86161.cpp +++ b/src/SensorManager/MAXM86161/MAXM86161.cpp @@ -146,12 +146,16 @@ int MAXM86161::read(ppg_sample *buffer, size_t buffer_capacity) { int status; int number_of_bytes; int fifo_items = 0; - int output_idx = -1; + size_t output_count = 0; status = _read_from_reg(REG_FIFO_DATA_COUNTER, fifo_items); if (status == 0){ - int items_to_read = MIN(fifo_items, (int)(buffer_capacity * _exposure_count)); - items_to_read -= items_to_read % _exposure_count; + int item_capacity = (int)(buffer_capacity * _exposure_count); + if (_pending_exposure_mask != 0) { + const int pending_items = __builtin_popcount((unsigned int)_pending_exposure_mask); + item_capacity -= pending_items; + } + int items_to_read = MIN(fifo_items, item_capacity); number_of_bytes = items_to_read * BYTES_PER_CH; status = _read_block(REG_FIFO_DATA, number_of_bytes, (uint8_t *) databuffer); @@ -167,22 +171,36 @@ int MAXM86161::read(ppg_sample *buffer, size_t buffer_capacity) { uint8_t tag = val >> 19; val = val & ((1 << 19) - 1); - //LOG_INF("tag: %i, val: %i", tag, val); + /* Picket-fence replacement tags 13-15 correspond to exposures 1-3. */ + if (tag >= 13 && tag <= 15) { + tag -= 12; + } if (tag == 1) { - if ((size_t)(output_idx + 1) >= buffer_capacity) { + memset(_pending_sample, 0, sizeof(_pending_sample)); + _pending_exposure_mask = 0; + } + if (tag == 0 || tag > _exposure_count || + (_pending_exposure_mask == 0 && tag != 1)) { + continue; + } + + _pending_sample[_exposure_output_indices[tag - 1]] = val; + _pending_exposure_mask |= (uint8_t)(1U << (tag - 1)); + + const uint8_t complete_mask = (uint8_t)((1U << _exposure_count) - 1U); + if (_pending_exposure_mask == complete_mask) { + if (output_count >= buffer_capacity) { break; } - output_idx++; - memset(buffer[output_idx], 0, sizeof(ppg_sample)); + memcpy(buffer[output_count], _pending_sample, sizeof(ppg_sample)); + output_count++; + _pending_exposure_mask = 0; } - if (tag == 0 || tag > _exposure_count || output_idx < 0) continue; - - buffer[output_idx][_exposure_output_indices[tag - 1]] = val; } } - return output_idx+1; + return (int)output_count; } @@ -352,6 +370,8 @@ int MAXM86161::set_exposure_count(uint8_t count) _exposure_count = count; memcpy(_exposure_output_indices, output_indices[count - 1], sizeof(_exposure_output_indices)); + memset(_pending_sample, 0, sizeof(_pending_sample)); + _pending_exposure_mask = 0; return 0; } diff --git a/src/SensorManager/MAXM86161/MAXM86161.h b/src/SensorManager/MAXM86161/MAXM86161.h index cc74bcad..560c1406 100644 --- a/src/SensorManager/MAXM86161/MAXM86161.h +++ b/src/SensorManager/MAXM86161/MAXM86161.h @@ -111,6 +111,8 @@ class MAXM86161 { uint8_t _exposure_count = LED_NUM; uint8_t _exposure_output_indices[LED_NUM] = { 1, 2, 0, 3 }; + ppg_sample _pending_sample = {}; + uint8_t _pending_exposure_mask = 0; // void _set_led_sequence(char sequence);