Improve high-rate PPG acquisition

src/SensorManager/MAXM86161/MAXM86161.cpp

@@ -1,10 +1,12 @@
 #include "MAXM86161.h"
+#include <errno.h>
+#include <string.h>
 #include <zephyr/kernel.h>
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(MAXM86161, 3);
 
-char databuffer[32*BYTES_PER_CH*LED_NUM];
+uint8_t databuffer[32*BYTES_PER_CH*LED_NUM];
 
 /*****************************************************************************/
 // Constructor
@@ -136,36 +138,69 @@ 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 output_idx = -1;
+    int fifo_items = 0;
+    size_t output_count = 0;
 
-    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 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;
 
-        (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];
 
             uint8_t tag = val >> 19;
             val = val & ((1 << 19) - 1);
 
-            //LOG_INF("tag: %i, val: %i", tag, val);
-
-            if (tag == 1) output_idx++;
-            if (tag > 6 || output_idx < 0) continue;
-
-            buffer[output_idx][tag-1] = val;
+            /* Picket-fence replacement tags 13-15 correspond to exposures 1-3. */
+            if (tag >= 13 && tag <= 15) {
+                tag -= 12;
+            }
+
+            if (tag == 1) {
+                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;
+                }
+                memcpy(buffer[output_count], _pending_sample, sizeof(ppg_sample));
+                output_count++;
+                _pending_exposure_mask = 0;
+            }
         }
     }
 
-    return output_idx+1;
+    return (int)output_count;
 }
 
 
@@ -176,7 +211,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 +223,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 +295,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 +308,73 @@ 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));
+    memset(_pending_sample, 0, sizeof(_pending_sample));
+    _pending_exposure_mask = 0;
+    return 0;
+}
+
 /*******************************************************************************/
 int MAXM86161::alc_on(void)
 {
@@ -389,7 +509,7 @@ int MAXM86161::_write_to_reg(int address, int value) {
 
     _i2c->release();
 
-    return 0;
+    return ret;
 }
 
 

src/SensorManager/MAXM86161/MAXM86161.h

@@ -6,6 +6,7 @@
 
 //#include <Wire.h>
 #include <TWIM.h>
+#include <stddef.h>
 #include <stdint.h>
 
 
@@ -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,11 @@ 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 };
+    ppg_sample _pending_sample = {};
+    uint8_t _pending_exposure_mask = 0;
+
     // void _set_led_sequence(char sequence);
 
     int _read_from_reg(int address, int &data);
@@ -118,12 +135,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 +156,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 +240,4 @@ class MAXM86161 {
 #define REG_FIFO_TAG_MASK   0x1F
 
 
-#endif /* __MAXM86161_H_*/
+#endif /* __MAXM86161_H_*/