File content as of revision 3:6d8153f8c92e:

#include "pixart_heart_rate.h"

#include "pah8series_api_c.h"

#include <limits>
#include <string.h>
#include <stdlib.h>


#define debug_printf(...)   \
    do { \
        if (m_debug_print_handler) \
            m_debug_print_handler(__VA_ARGS__); \
    } while (0)

#define log_printf(...) debug_printf(__VA_ARGS__)


namespace pixart {
    
    
    class pah_hr_data : public pah8series_data_t {};
    
    
    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_debug_print_handler(NULL)
        , 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::enable_debug_print(DEBUG_PRINT_HANDLE handler)
    {
        m_debug_print_handler = handler;
    }
    
    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;
        
        pah_hr_data 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;
        
        if (m_debug_print_handler)
            log_pah8series_data(&pah8series_data);
        
        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);
        
        debug_printf("ret = %d, hr = %d, hr_trust_level = %d\n", ret, (int)hr, 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;
    }
    
    void heart_rate::log_pah8series_data(const pah_hr_data *pxialg_data)
    {
        int i = 0;
        uint32_t *ppg_data = (uint32_t *)pxialg_data->ppg_data;
        int16_t *mems_data = pxialg_data->mems_data;
        int data_num = pxialg_data->nf_ppg_channel * pxialg_data->nf_ppg_per_channel;
        
        log_printf("Frame Count, %d \n", pxialg_data->frame_count);
        log_printf("Time, %d \n", pxialg_data->time);
        log_printf("PPG, %d, %d, ", pxialg_data->touch_flag, data_num);
        for (i = 0; i < data_num; ++i)
        {
            log_printf("%d, ", *ppg_data);
            ppg_data++;
        }
        log_printf("\n");
        log_printf("MEMS, %d, ", pxialg_data->nf_mems);
        for (i = 0; i < pxialg_data->nf_mems * 3; ++i)
        {
            log_printf("%d, ", *mems_data);
            mems_data++;
        }
        log_printf("\n");
    }
    

}