Bell Huang / pixart_heart_rate

class heart_rate

pixart_heart_rate.cpp@0:31d7988294aa, 2017-10-27 (annotated)

Committer:
bell_huang
Date:
Fri Oct 27 05:55:57 2017 +0000
Revision:
0:31d7988294aa
Child:
1:e9f4fb198b63
Init version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bell_huang 0:31d7988294aa 1 #include "pixart_heart_rate.h"
bell_huang 0:31d7988294aa 2
bell_huang 0:31d7988294aa 3 #include "pah8series_api_c.h"
bell_huang 0:31d7988294aa 4
bell_huang 0:31d7988294aa 5 #include <limits>
bell_huang 0:31d7988294aa 6 #include <string.h>
bell_huang 0:31d7988294aa 7 #include <stdlib.h>
bell_huang 0:31d7988294aa 8
bell_huang 0:31d7988294aa 9
bell_huang 0:31d7988294aa 10 namespace pixart {
bell_huang 0:31d7988294aa 11
bell_huang 0:31d7988294aa 12
bell_huang 0:31d7988294aa 13 const float heart_rate::GRAVITY_OF_EARTH = 9.80665f;
bell_huang 0:31d7988294aa 14 const size_t heart_rate::PPG_SYNC_COUNT = 20;
bell_huang 0:31d7988294aa 15 const size_t heart_rate::ACCEL_AXIS_NUM = 3;
bell_huang 0:31d7988294aa 16
bell_huang 0:31d7988294aa 17 heart_rate::heart_rate(TIME_TO_MILLISEC_HANDLE time_to_millisec_handler, size_t ppg_max_sample_num, size_t accel_max_sample_num)
bell_huang 0:31d7988294aa 18 : PPG_MAX_SAMPLE_NUM(ppg_max_sample_num)
bell_huang 0:31d7988294aa 19 , PPG_CH_NUM(2)
bell_huang 0:31d7988294aa 20 , ACCEL_MAX_SAMPLE_NUM(accel_max_sample_num)
bell_huang 0:31d7988294aa 21 , m_time_to_millisec_handler(time_to_millisec_handler)
bell_huang 0:31d7988294aa 22 , m_alg_buffer(NULL)
bell_huang 0:31d7988294aa 23 , m_gravity_mode(gravity_4g)
bell_huang 0:31d7988294aa 24 , m_last_report_timestamp(0)
bell_huang 0:31d7988294aa 25 , m_frame_count(0)
bell_huang 0:31d7988294aa 26 {
bell_huang 0:31d7988294aa 27 }
bell_huang 0:31d7988294aa 28
bell_huang 0:31d7988294aa 29 heart_rate::~heart_rate()
bell_huang 0:31d7988294aa 30 {
bell_huang 0:31d7988294aa 31 pah8series_close();
bell_huang 0:31d7988294aa 32
bell_huang 0:31d7988294aa 33 if (m_alg_buffer)
bell_huang 0:31d7988294aa 34 free(m_alg_buffer);
bell_huang 0:31d7988294aa 35 }
bell_huang 0:31d7988294aa 36
bell_huang 0:31d7988294aa 37 bool heart_rate::init(gravity_mode gravity)
bell_huang 0:31d7988294aa 38 {
bell_huang 0:31d7988294aa 39 if (!m_alg_buffer)
bell_huang 0:31d7988294aa 40 m_alg_buffer = malloc(pah8series_query_open_size());
bell_huang 0:31d7988294aa 41
bell_huang 0:31d7988294aa 42 if (!m_alg_buffer)
bell_huang 0:31d7988294aa 43 return false;
bell_huang 0:31d7988294aa 44
bell_huang 0:31d7988294aa 45 uint8_t ret = pah8series_open(m_alg_buffer);
bell_huang 0:31d7988294aa 46 if (ret != MSG_SUCCESS)
bell_huang 0:31d7988294aa 47 return false;
bell_huang 0:31d7988294aa 48
bell_huang 0:31d7988294aa 49 pah8series_set_param(PAH8SERIES_PARAM_IDX_PPG_CH_NUM, (float)PPG_CH_NUM);
bell_huang 0:31d7988294aa 50 pah8series_set_param(PAH8SERIES_PARAM_IDX_HAS_IR_CH, 0.0f);
bell_huang 0:31d7988294aa 51
bell_huang 0:31d7988294aa 52 float gsensor_mode = (float)gravity_to_hr_alg_gsensor_mode(gravity);
bell_huang 0:31d7988294aa 53 pah8series_set_param(PAH8SERIES_PARAM_IDX_GSENSOR_MODE, gsensor_mode);
bell_huang 0:31d7988294aa 54
bell_huang 0:31d7988294aa 55 m_gravity_mode = gravity;
bell_huang 0:31d7988294aa 56
bell_huang 0:31d7988294aa 57 return true;
bell_huang 0:31d7988294aa 58 }
bell_huang 0:31d7988294aa 59
bell_huang 0:31d7988294aa 60 void heart_rate::reset()
bell_huang 0:31d7988294aa 61 {
bell_huang 0:31d7988294aa 62 m_ppg_sample_queue.clear();
bell_huang 0:31d7988294aa 63 m_accel_sample_queue.clear();
bell_huang 0:31d7988294aa 64 m_last_report_timestamp = 0;
bell_huang 0:31d7988294aa 65 m_frame_count = 0;
bell_huang 0:31d7988294aa 66 memset(&m_result, 0, sizeof(m_result));
bell_huang 0:31d7988294aa 67 }
bell_huang 0:31d7988294aa 68
bell_huang 0:31d7988294aa 69 bool heart_rate::set_ppg_sample(const ppg_sample &ppg_sample)
bell_huang 0:31d7988294aa 70 {
bell_huang 0:31d7988294aa 71 // overflow
bell_huang 0:31d7988294aa 72 if (m_ppg_sample_queue.size() + 1 > PPG_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 73 return false;
bell_huang 0:31d7988294aa 74
bell_huang 0:31d7988294aa 75 m_ppg_sample_queue.push_back(ppg_sample);
bell_huang 0:31d7988294aa 76
bell_huang 0:31d7988294aa 77 flush();
bell_huang 0:31d7988294aa 78
bell_huang 0:31d7988294aa 79 return true;
bell_huang 0:31d7988294aa 80 }
bell_huang 0:31d7988294aa 81
bell_huang 0:31d7988294aa 82 bool heart_rate::set_accel_sample(const accel_sample &accel_sample)
bell_huang 0:31d7988294aa 83 {
bell_huang 0:31d7988294aa 84 // overflow
bell_huang 0:31d7988294aa 85 if (m_accel_sample_queue.size() + 1 > ACCEL_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 86 return false;
bell_huang 0:31d7988294aa 87
bell_huang 0:31d7988294aa 88 m_accel_sample_queue.push_back(accel_sample);
bell_huang 0:31d7988294aa 89
bell_huang 0:31d7988294aa 90 return true;
bell_huang 0:31d7988294aa 91 }
bell_huang 0:31d7988294aa 92
bell_huang 0:31d7988294aa 93 const heart_rate::result& heart_rate::get_result() const
bell_huang 0:31d7988294aa 94 {
bell_huang 0:31d7988294aa 95 return m_result;
bell_huang 0:31d7988294aa 96 }
bell_huang 0:31d7988294aa 97
bell_huang 0:31d7988294aa 98 int heart_rate::gravity_to_hr_alg_gsensor_mode(gravity_mode gravity)
bell_huang 0:31d7988294aa 99 {
bell_huang 0:31d7988294aa 100 int gsensor_mode = 0;
bell_huang 0:31d7988294aa 101
bell_huang 0:31d7988294aa 102 switch (gravity)
bell_huang 0:31d7988294aa 103 {
bell_huang 0:31d7988294aa 104 case gravity_2g:
bell_huang 0:31d7988294aa 105 gsensor_mode = 0;
bell_huang 0:31d7988294aa 106 break;
bell_huang 0:31d7988294aa 107
bell_huang 0:31d7988294aa 108 case gravity_4g:
bell_huang 0:31d7988294aa 109 gsensor_mode = 1;
bell_huang 0:31d7988294aa 110 break;
bell_huang 0:31d7988294aa 111
bell_huang 0:31d7988294aa 112 case gravity_8g:
bell_huang 0:31d7988294aa 113 gsensor_mode = 2;
bell_huang 0:31d7988294aa 114 break;
bell_huang 0:31d7988294aa 115
bell_huang 0:31d7988294aa 116 case gravity_16g:
bell_huang 0:31d7988294aa 117 default:
bell_huang 0:31d7988294aa 118 gsensor_mode = 3;
bell_huang 0:31d7988294aa 119 break;
bell_huang 0:31d7988294aa 120 }
bell_huang 0:31d7988294aa 121
bell_huang 0:31d7988294aa 122 return gsensor_mode;
bell_huang 0:31d7988294aa 123 }
bell_huang 0:31d7988294aa 124
bell_huang 0:31d7988294aa 125 int16_t heart_rate::gravity_to_int16(gravity_mode gravity, float value)
bell_huang 0:31d7988294aa 126 {
bell_huang 0:31d7988294aa 127 typedef int16_t result_type;
bell_huang 0:31d7988294aa 128 static const result_type MAX = std::numeric_limits<result_type>::max();
bell_huang 0:31d7988294aa 129 static const result_type MIN = std::numeric_limits<result_type>::min();
bell_huang 0:31d7988294aa 130
bell_huang 0:31d7988294aa 131 float result = (value * MAX) / ((float)gravity * GRAVITY_OF_EARTH);
bell_huang 0:31d7988294aa 132 if (result > MAX)
bell_huang 0:31d7988294aa 133 return MAX;
bell_huang 0:31d7988294aa 134 else if (result < MIN)
bell_huang 0:31d7988294aa 135 return MIN;
bell_huang 0:31d7988294aa 136 return (result_type)result;
bell_huang 0:31d7988294aa 137 }
bell_huang 0:31d7988294aa 138
bell_huang 0:31d7988294aa 139 void heart_rate::flush()
bell_huang 0:31d7988294aa 140 {
bell_huang 0:31d7988294aa 141 // Get sync timestamp
bell_huang 0:31d7988294aa 142 if (m_ppg_sample_queue.size() < PPG_SYNC_COUNT)
bell_huang 0:31d7988294aa 143 return;
bell_huang 0:31d7988294aa 144 uint64_t sync_timestamp = m_ppg_sample_queue[PPG_SYNC_COUNT - 1].timestamp;
bell_huang 0:31d7988294aa 145
bell_huang 0:31d7988294aa 146 // Ensure accel sensor has data timestamp later than sync_timestamp
bell_huang 0:31d7988294aa 147 if (m_ppg_sample_queue.size() < PPG_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 148 {
bell_huang 0:31d7988294aa 149 if (m_accel_sample_queue.empty() || (m_accel_sample_queue.back().timestamp < sync_timestamp))
bell_huang 0:31d7988294aa 150 return;
bell_huang 0:31d7988294aa 151 }
bell_huang 0:31d7988294aa 152
bell_huang 0:31d7988294aa 153 // Flush PPG
bell_huang 0:31d7988294aa 154 size_t flush_ppg_sample_count = PPG_SYNC_COUNT;
bell_huang 0:31d7988294aa 155 std::vector<int32_t> hr_ppg_data(flush_ppg_sample_count * PPG_CH_NUM);
bell_huang 0:31d7988294aa 156 for (size_t i = 0; i < flush_ppg_sample_count; ++i)
bell_huang 0:31d7988294aa 157 {
bell_huang 0:31d7988294aa 158 hr_ppg_data[i * PPG_CH_NUM + 0] = m_ppg_sample_queue[i].ch1;
bell_huang 0:31d7988294aa 159 hr_ppg_data[i * PPG_CH_NUM + 1] = m_ppg_sample_queue[i].ch2;
bell_huang 0:31d7988294aa 160 }
bell_huang 0:31d7988294aa 161 uint8_t touch_flag = m_ppg_sample_queue[flush_ppg_sample_count - 1].is_touched;
bell_huang 0:31d7988294aa 162 uint64_t ppg_timestamp = m_ppg_sample_queue[flush_ppg_sample_count - 1].timestamp;
bell_huang 0:31d7988294aa 163 if (m_last_report_timestamp == 0)
bell_huang 0:31d7988294aa 164 m_last_report_timestamp = m_ppg_sample_queue.front().timestamp;
bell_huang 0:31d7988294aa 165 std::deque<ppg_sample>::iterator flush_ppg_end_iter(m_ppg_sample_queue.begin());
bell_huang 0:31d7988294aa 166 std::advance(flush_ppg_end_iter, flush_ppg_sample_count);
bell_huang 0:31d7988294aa 167 m_ppg_sample_queue.erase(m_ppg_sample_queue.begin(), flush_ppg_end_iter);
bell_huang 0:31d7988294aa 168
bell_huang 0:31d7988294aa 169 // Flush ACCEL
bell_huang 0:31d7988294aa 170 size_t flush_accel_sample_count = 0;
bell_huang 0:31d7988294aa 171 for (size_t i = 0; i < m_accel_sample_queue.size(); ++i)
bell_huang 0:31d7988294aa 172 {
bell_huang 0:31d7988294aa 173 if (m_accel_sample_queue[i].timestamp <= sync_timestamp)
bell_huang 0:31d7988294aa 174 ++flush_accel_sample_count;
bell_huang 0:31d7988294aa 175 else
bell_huang 0:31d7988294aa 176 break;
bell_huang 0:31d7988294aa 177 }
bell_huang 0:31d7988294aa 178 std::vector<int16_t> hr_accel_data(flush_accel_sample_count * ACCEL_AXIS_NUM);
bell_huang 0:31d7988294aa 179 for (size_t i = 0; i < flush_accel_sample_count; ++i)
bell_huang 0:31d7988294aa 180 {
bell_huang 0:31d7988294aa 181 hr_accel_data[i * ACCEL_AXIS_NUM + 0] = (int16_t)gravity_to_int16(m_gravity_mode, m_accel_sample_queue[i].x);
bell_huang 0:31d7988294aa 182 hr_accel_data[i * ACCEL_AXIS_NUM + 1] = (int16_t)gravity_to_int16(m_gravity_mode, m_accel_sample_queue[i].y);
bell_huang 0:31d7988294aa 183 hr_accel_data[i * ACCEL_AXIS_NUM + 2] = (int16_t)gravity_to_int16(m_gravity_mode, m_accel_sample_queue[i].z);
bell_huang 0:31d7988294aa 184 }
bell_huang 0:31d7988294aa 185 std::deque<accel_sample>::iterator flush_accel_end_iter(m_accel_sample_queue.begin());
bell_huang 0:31d7988294aa 186 std::advance(flush_accel_end_iter, flush_accel_sample_count);
bell_huang 0:31d7988294aa 187 m_accel_sample_queue.erase(m_accel_sample_queue.begin(), flush_accel_end_iter);
bell_huang 0:31d7988294aa 188
bell_huang 0:31d7988294aa 189 // Duration
bell_huang 0:31d7988294aa 190 uint32_t time_ms = (uint32_t)m_time_to_millisec_handler(ppg_timestamp - m_last_report_timestamp);
bell_huang 0:31d7988294aa 191 m_last_report_timestamp = ppg_timestamp;
bell_huang 0:31d7988294aa 192
bell_huang 0:31d7988294aa 193 pah8series_data_t pah8series_data;
bell_huang 0:31d7988294aa 194 pah8series_data.frame_count = m_frame_count;
bell_huang 0:31d7988294aa 195 pah8series_data.time = time_ms;
bell_huang 0:31d7988294aa 196 pah8series_data.touch_flag = touch_flag;
bell_huang 0:31d7988294aa 197 pah8series_data.nf_ppg_channel = PPG_CH_NUM;
bell_huang 0:31d7988294aa 198 pah8series_data.nf_ppg_per_channel = flush_ppg_sample_count;
bell_huang 0:31d7988294aa 199 pah8series_data.ppg_data = &hr_ppg_data[0];
bell_huang 0:31d7988294aa 200 pah8series_data.nf_mems = flush_accel_sample_count;
bell_huang 0:31d7988294aa 201 pah8series_data.mems_data = &hr_accel_data[0];
bell_huang 0:31d7988294aa 202 ++m_frame_count;
bell_huang 0:31d7988294aa 203
bell_huang 0:31d7988294aa 204 uint8_t ret = pah8series_entrance(&pah8series_data);
bell_huang 0:31d7988294aa 205
bell_huang 0:31d7988294aa 206 float hr = 0.0f;
bell_huang 0:31d7988294aa 207 pah8series_get_hr(&hr);
bell_huang 0:31d7988294aa 208
bell_huang 0:31d7988294aa 209 int hr_trust_level = 0;
bell_huang 0:31d7988294aa 210 pah8series_get_hr_trust_level(&hr_trust_level);
bell_huang 0:31d7988294aa 211
bell_huang 0:31d7988294aa 212 if (ret == MSG_SUCCESS
bell_huang 0:31d7988294aa 213 || ret == MSG_HR_READY)
bell_huang 0:31d7988294aa 214 m_result.ret = ret_success;
bell_huang 0:31d7988294aa 215 else if (ret == MSG_NO_TOUCH)
bell_huang 0:31d7988294aa 216 m_result.ret = ret_no_touch;
bell_huang 0:31d7988294aa 217 else if (ret == MSG_SIGNAL_POOR)
bell_huang 0:31d7988294aa 218 m_result.ret = ret_signal_poor;
bell_huang 0:31d7988294aa 219 else
bell_huang 0:31d7988294aa 220 m_result.ret = ret_fail;
bell_huang 0:31d7988294aa 221 m_result.hr = hr;
bell_huang 0:31d7988294aa 222 m_result.hr_trust_level = hr_trust_level;
bell_huang 0:31d7988294aa 223 }
bell_huang 0:31d7988294aa 224
bell_huang 0:31d7988294aa 225 }
bell_huang 0:31d7988294aa 226