Diff: pixart_heart_rate.cpp

Revision:

0:31d7988294aa

Child:

1:e9f4fb198b63

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pixart_heart_rate.cpp	Fri Oct 27 05:55:57 2017 +0000
@@ -0,0 +1,226 @@
+#include "pixart_heart_rate.h"
+
+#include "pah8series_api_c.h"
+
+#include <limits>
+#include <string.h>
+#include <stdlib.h>
+
+
+namespace pixart {
+    
+    
+    const float     heart_rate::GRAVITY_OF_EARTH = 9.80665f;
+    const size_t    heart_rate::PPG_SYNC_COUNT = 20;
+    const size_t    heart_rate::ACCEL_AXIS_NUM = 3;
+    
+    heart_rate::heart_rate(TIME_TO_MILLISEC_HANDLE time_to_millisec_handler, size_t ppg_max_sample_num, size_t accel_max_sample_num)
+        : PPG_MAX_SAMPLE_NUM(ppg_max_sample_num)
+        , PPG_CH_NUM(2)
+        , ACCEL_MAX_SAMPLE_NUM(accel_max_sample_num)
+        , m_time_to_millisec_handler(time_to_millisec_handler)
+        , m_alg_buffer(NULL)
+        , m_gravity_mode(gravity_4g)
+        , m_last_report_timestamp(0)
+        , m_frame_count(0)
+    {
+    }
+    
+    heart_rate::~heart_rate()
+    {
+        pah8series_close();
+        
+        if (m_alg_buffer)
+            free(m_alg_buffer);
+    }
+    
+    bool heart_rate::init(gravity_mode gravity)
+    {
+        if (!m_alg_buffer)
+            m_alg_buffer = malloc(pah8series_query_open_size());
+        
+        if (!m_alg_buffer)
+            return false;
+            
+        uint8_t ret = pah8series_open(m_alg_buffer);
+        if (ret != MSG_SUCCESS)
+            return false;
+            
+        pah8series_set_param(PAH8SERIES_PARAM_IDX_PPG_CH_NUM, (float)PPG_CH_NUM);
+        pah8series_set_param(PAH8SERIES_PARAM_IDX_HAS_IR_CH, 0.0f);
+        
+        float gsensor_mode = (float)gravity_to_hr_alg_gsensor_mode(gravity);
+        pah8series_set_param(PAH8SERIES_PARAM_IDX_GSENSOR_MODE, gsensor_mode);
+        
+        m_gravity_mode = gravity;
+        
+        return true;
+    }
+    
+    void heart_rate::reset()
+    {
+        m_ppg_sample_queue.clear();
+        m_accel_sample_queue.clear();
+        m_last_report_timestamp = 0;
+        m_frame_count = 0;
+        memset(&m_result, 0, sizeof(m_result));
+    }
+    
+    bool heart_rate::set_ppg_sample(const ppg_sample &ppg_sample)
+    {
+        // overflow
+        if (m_ppg_sample_queue.size() + 1 > PPG_MAX_SAMPLE_NUM)
+            return false;
+            
+        m_ppg_sample_queue.push_back(ppg_sample);
+        
+        flush();
+        
+        return true;
+    }
+    
+    bool heart_rate::set_accel_sample(const accel_sample &accel_sample)
+    {
+        // overflow
+        if (m_accel_sample_queue.size() + 1 > ACCEL_MAX_SAMPLE_NUM)
+            return false;
+            
+        m_accel_sample_queue.push_back(accel_sample);
+        
+        return true;
+    }
+    
+    const heart_rate::result& heart_rate::get_result() const
+    {
+        return m_result;
+    }
+    
+    int heart_rate::gravity_to_hr_alg_gsensor_mode(gravity_mode gravity)
+    {
+        int gsensor_mode = 0;
+    
+        switch (gravity)
+        {
+        case gravity_2g:
+            gsensor_mode = 0;
+            break;
+    
+        case gravity_4g:
+            gsensor_mode = 1;
+            break;
+    
+        case gravity_8g:
+            gsensor_mode = 2;
+            break;
+    
+        case gravity_16g:
+        default:
+            gsensor_mode = 3;
+            break;
+        }
+    
+        return gsensor_mode;
+    }
+    
+    int16_t heart_rate::gravity_to_int16(gravity_mode gravity, float value)
+    {
+        typedef int16_t result_type;
+        static const result_type MAX = std::numeric_limits<result_type>::max();
+        static const result_type MIN = std::numeric_limits<result_type>::min();
+        
+        float result = (value  * MAX) / ((float)gravity * GRAVITY_OF_EARTH);
+        if (result > MAX)
+            return MAX;
+        else if (result < MIN)
+            return MIN;
+        return (result_type)result;
+    }
+
+    void heart_rate::flush()
+    {
+        // Get sync timestamp
+        if (m_ppg_sample_queue.size() < PPG_SYNC_COUNT)
+            return;
+        uint64_t sync_timestamp = m_ppg_sample_queue[PPG_SYNC_COUNT - 1].timestamp;
+        
+        // Ensure accel sensor has data timestamp later than sync_timestamp
+        if (m_ppg_sample_queue.size() < PPG_MAX_SAMPLE_NUM)
+        {
+            if (m_accel_sample_queue.empty() || (m_accel_sample_queue.back().timestamp < sync_timestamp))
+                return;
+        }
+        
+        // Flush PPG
+        size_t flush_ppg_sample_count = PPG_SYNC_COUNT;
+        std::vector<int32_t> hr_ppg_data(flush_ppg_sample_count * PPG_CH_NUM);
+        for (size_t i = 0; i < flush_ppg_sample_count; ++i)
+        {
+            hr_ppg_data[i * PPG_CH_NUM + 0] = m_ppg_sample_queue[i].ch1;
+            hr_ppg_data[i * PPG_CH_NUM + 1] = m_ppg_sample_queue[i].ch2;
+        }
+        uint8_t touch_flag = m_ppg_sample_queue[flush_ppg_sample_count - 1].is_touched;
+        uint64_t ppg_timestamp = m_ppg_sample_queue[flush_ppg_sample_count - 1].timestamp;
+        if (m_last_report_timestamp == 0)
+            m_last_report_timestamp = m_ppg_sample_queue.front().timestamp;
+        std::deque<ppg_sample>::iterator flush_ppg_end_iter(m_ppg_sample_queue.begin());
+        std::advance(flush_ppg_end_iter, flush_ppg_sample_count);
+        m_ppg_sample_queue.erase(m_ppg_sample_queue.begin(), flush_ppg_end_iter);
+        
+        // Flush ACCEL
+        size_t flush_accel_sample_count = 0;
+        for (size_t i = 0; i < m_accel_sample_queue.size(); ++i)
+        {
+            if (m_accel_sample_queue[i].timestamp <= sync_timestamp)
+                ++flush_accel_sample_count;
+            else
+                break;
+        }
+        std::vector<int16_t> hr_accel_data(flush_accel_sample_count * ACCEL_AXIS_NUM);
+        for (size_t i = 0; i < flush_accel_sample_count; ++i)
+        {
+            hr_accel_data[i * ACCEL_AXIS_NUM + 0] = (int16_t)gravity_to_int16(m_gravity_mode, m_accel_sample_queue[i].x);
+            hr_accel_data[i * ACCEL_AXIS_NUM + 1] = (int16_t)gravity_to_int16(m_gravity_mode, m_accel_sample_queue[i].y);
+            hr_accel_data[i * ACCEL_AXIS_NUM + 2] = (int16_t)gravity_to_int16(m_gravity_mode, m_accel_sample_queue[i].z);
+        }
+        std::deque<accel_sample>::iterator flush_accel_end_iter(m_accel_sample_queue.begin());
+        std::advance(flush_accel_end_iter, flush_accel_sample_count);
+        m_accel_sample_queue.erase(m_accel_sample_queue.begin(), flush_accel_end_iter);
+        
+        // Duration
+        uint32_t time_ms = (uint32_t)m_time_to_millisec_handler(ppg_timestamp - m_last_report_timestamp);
+        m_last_report_timestamp = ppg_timestamp;
+        
+        pah8series_data_t pah8series_data;
+        pah8series_data.frame_count        = m_frame_count;
+        pah8series_data.time               = time_ms;
+        pah8series_data.touch_flag         = touch_flag;
+        pah8series_data.nf_ppg_channel     = PPG_CH_NUM;
+        pah8series_data.nf_ppg_per_channel = flush_ppg_sample_count;
+        pah8series_data.ppg_data           = &hr_ppg_data[0];
+        pah8series_data.nf_mems            = flush_accel_sample_count;
+        pah8series_data.mems_data          = &hr_accel_data[0];
+        ++m_frame_count;
+        
+        uint8_t ret = pah8series_entrance(&pah8series_data);
+        
+        float hr = 0.0f;
+        pah8series_get_hr(&hr);
+        
+        int hr_trust_level = 0;
+        pah8series_get_hr_trust_level(&hr_trust_level);
+        
+        if (ret == MSG_SUCCESS
+         || ret == MSG_HR_READY)
+            m_result.ret = ret_success;
+        else if (ret == MSG_NO_TOUCH)
+            m_result.ret = ret_no_touch;
+        else if (ret == MSG_SIGNAL_POOR)
+            m_result.ret = ret_signal_poor;
+        else
+            m_result.ret = ret_fail;
+        m_result.hr = hr;
+        m_result.hr_trust_level = hr_trust_level;
+    }
+
+}
+