Bell Huang / pixart_heart_rate

class heart_rate

pixart_heart_rate.cpp@4:90dfc6d7461b, 2017-10-27 (annotated)

Committer:
bell_huang
Date:
Fri Oct 27 07:00:07 2017 +0000
Revision:
4:90dfc6d7461b
Parent:
3:6d8153f8c92e
Child:
5:d172ac7bfd77
@@@

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 3:6d8153f8c92e 10 #define debug_printf(...) \
bell_huang 3:6d8153f8c92e 11 do { \
bell_huang 3:6d8153f8c92e 12 if (m_debug_print_handler) \
bell_huang 3:6d8153f8c92e 13 m_debug_print_handler(__VA_ARGS__); \
bell_huang 3:6d8153f8c92e 14 } while (0)
bell_huang 3:6d8153f8c92e 15
bell_huang 3:6d8153f8c92e 16 #define log_printf(...) debug_printf(__VA_ARGS__)
bell_huang 1:e9f4fb198b63 17
bell_huang 1:e9f4fb198b63 18
bell_huang 0:31d7988294aa 19 namespace pixart {
bell_huang 0:31d7988294aa 20
bell_huang 0:31d7988294aa 21
bell_huang 1:e9f4fb198b63 22 class pah_hr_data : public pah8series_data_t {};
bell_huang 1:e9f4fb198b63 23
bell_huang 1:e9f4fb198b63 24
bell_huang 0:31d7988294aa 25 const float heart_rate::GRAVITY_OF_EARTH = 9.80665f;
bell_huang 0:31d7988294aa 26 const size_t heart_rate::PPG_SYNC_COUNT = 20;
bell_huang 0:31d7988294aa 27 const size_t heart_rate::ACCEL_AXIS_NUM = 3;
bell_huang 0:31d7988294aa 28
bell_huang 0:31d7988294aa 29 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 30 : PPG_MAX_SAMPLE_NUM(ppg_max_sample_num)
bell_huang 0:31d7988294aa 31 , PPG_CH_NUM(2)
bell_huang 0:31d7988294aa 32 , ACCEL_MAX_SAMPLE_NUM(accel_max_sample_num)
bell_huang 2:5cb0e405a24c 33 , m_debug_print_handler(NULL)
bell_huang 0:31d7988294aa 34 , m_time_to_millisec_handler(time_to_millisec_handler)
bell_huang 0:31d7988294aa 35 , m_alg_buffer(NULL)
bell_huang 0:31d7988294aa 36 , m_gravity_mode(gravity_4g)
bell_huang 0:31d7988294aa 37 , m_last_report_timestamp(0)
bell_huang 0:31d7988294aa 38 , m_frame_count(0)
bell_huang 0:31d7988294aa 39 {
bell_huang 0:31d7988294aa 40 }
bell_huang 0:31d7988294aa 41
bell_huang 0:31d7988294aa 42 heart_rate::~heart_rate()
bell_huang 0:31d7988294aa 43 {
bell_huang 0:31d7988294aa 44 pah8series_close();
bell_huang 0:31d7988294aa 45
bell_huang 0:31d7988294aa 46 if (m_alg_buffer)
bell_huang 0:31d7988294aa 47 free(m_alg_buffer);
bell_huang 0:31d7988294aa 48 }
bell_huang 0:31d7988294aa 49
bell_huang 0:31d7988294aa 50 bool heart_rate::init(gravity_mode gravity)
bell_huang 0:31d7988294aa 51 {
bell_huang 0:31d7988294aa 52 if (!m_alg_buffer)
bell_huang 0:31d7988294aa 53 m_alg_buffer = malloc(pah8series_query_open_size());
bell_huang 0:31d7988294aa 54
bell_huang 0:31d7988294aa 55 if (!m_alg_buffer)
bell_huang 0:31d7988294aa 56 return false;
bell_huang 0:31d7988294aa 57
bell_huang 0:31d7988294aa 58 uint8_t ret = pah8series_open(m_alg_buffer);
bell_huang 0:31d7988294aa 59 if (ret != MSG_SUCCESS)
bell_huang 0:31d7988294aa 60 return false;
bell_huang 0:31d7988294aa 61
bell_huang 0:31d7988294aa 62 pah8series_set_param(PAH8SERIES_PARAM_IDX_PPG_CH_NUM, (float)PPG_CH_NUM);
bell_huang 0:31d7988294aa 63 pah8series_set_param(PAH8SERIES_PARAM_IDX_HAS_IR_CH, 0.0f);
bell_huang 0:31d7988294aa 64
bell_huang 0:31d7988294aa 65 float gsensor_mode = (float)gravity_to_hr_alg_gsensor_mode(gravity);
bell_huang 0:31d7988294aa 66 pah8series_set_param(PAH8SERIES_PARAM_IDX_GSENSOR_MODE, gsensor_mode);
bell_huang 0:31d7988294aa 67
bell_huang 4:90dfc6d7461b 68 log_pah8series_data_header(PPG_CH_NUM, 0, gsensor_mode);
bell_huang 4:90dfc6d7461b 69
bell_huang 0:31d7988294aa 70 m_gravity_mode = gravity;
bell_huang 0:31d7988294aa 71
bell_huang 0:31d7988294aa 72 return true;
bell_huang 0:31d7988294aa 73 }
bell_huang 0:31d7988294aa 74
bell_huang 2:5cb0e405a24c 75 void heart_rate::enable_debug_print(DEBUG_PRINT_HANDLE handler)
bell_huang 1:e9f4fb198b63 76 {
bell_huang 2:5cb0e405a24c 77 m_debug_print_handler = handler;
bell_huang 1:e9f4fb198b63 78 }
bell_huang 1:e9f4fb198b63 79
bell_huang 0:31d7988294aa 80 void heart_rate::reset()
bell_huang 0:31d7988294aa 81 {
bell_huang 0:31d7988294aa 82 m_ppg_sample_queue.clear();
bell_huang 0:31d7988294aa 83 m_accel_sample_queue.clear();
bell_huang 0:31d7988294aa 84 m_last_report_timestamp = 0;
bell_huang 0:31d7988294aa 85 m_frame_count = 0;
bell_huang 0:31d7988294aa 86 memset(&m_result, 0, sizeof(m_result));
bell_huang 0:31d7988294aa 87 }
bell_huang 0:31d7988294aa 88
bell_huang 0:31d7988294aa 89 bool heart_rate::set_ppg_sample(const ppg_sample &ppg_sample)
bell_huang 0:31d7988294aa 90 {
bell_huang 0:31d7988294aa 91 // overflow
bell_huang 0:31d7988294aa 92 if (m_ppg_sample_queue.size() + 1 > PPG_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 93 return false;
bell_huang 0:31d7988294aa 94
bell_huang 0:31d7988294aa 95 m_ppg_sample_queue.push_back(ppg_sample);
bell_huang 0:31d7988294aa 96
bell_huang 0:31d7988294aa 97 flush();
bell_huang 0:31d7988294aa 98
bell_huang 0:31d7988294aa 99 return true;
bell_huang 0:31d7988294aa 100 }
bell_huang 0:31d7988294aa 101
bell_huang 0:31d7988294aa 102 bool heart_rate::set_accel_sample(const accel_sample &accel_sample)
bell_huang 0:31d7988294aa 103 {
bell_huang 0:31d7988294aa 104 // overflow
bell_huang 0:31d7988294aa 105 if (m_accel_sample_queue.size() + 1 > ACCEL_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 106 return false;
bell_huang 0:31d7988294aa 107
bell_huang 0:31d7988294aa 108 m_accel_sample_queue.push_back(accel_sample);
bell_huang 0:31d7988294aa 109
bell_huang 0:31d7988294aa 110 return true;
bell_huang 0:31d7988294aa 111 }
bell_huang 0:31d7988294aa 112
bell_huang 0:31d7988294aa 113 const heart_rate::result& heart_rate::get_result() const
bell_huang 0:31d7988294aa 114 {
bell_huang 0:31d7988294aa 115 return m_result;
bell_huang 0:31d7988294aa 116 }
bell_huang 0:31d7988294aa 117
bell_huang 0:31d7988294aa 118 int heart_rate::gravity_to_hr_alg_gsensor_mode(gravity_mode gravity)
bell_huang 0:31d7988294aa 119 {
bell_huang 0:31d7988294aa 120 int gsensor_mode = 0;
bell_huang 0:31d7988294aa 121
bell_huang 0:31d7988294aa 122 switch (gravity)
bell_huang 0:31d7988294aa 123 {
bell_huang 0:31d7988294aa 124 case gravity_2g:
bell_huang 0:31d7988294aa 125 gsensor_mode = 0;
bell_huang 0:31d7988294aa 126 break;
bell_huang 0:31d7988294aa 127
bell_huang 0:31d7988294aa 128 case gravity_4g:
bell_huang 0:31d7988294aa 129 gsensor_mode = 1;
bell_huang 0:31d7988294aa 130 break;
bell_huang 0:31d7988294aa 131
bell_huang 0:31d7988294aa 132 case gravity_8g:
bell_huang 0:31d7988294aa 133 gsensor_mode = 2;
bell_huang 0:31d7988294aa 134 break;
bell_huang 0:31d7988294aa 135
bell_huang 0:31d7988294aa 136 case gravity_16g:
bell_huang 0:31d7988294aa 137 default:
bell_huang 0:31d7988294aa 138 gsensor_mode = 3;
bell_huang 0:31d7988294aa 139 break;
bell_huang 0:31d7988294aa 140 }
bell_huang 0:31d7988294aa 141
bell_huang 0:31d7988294aa 142 return gsensor_mode;
bell_huang 0:31d7988294aa 143 }
bell_huang 0:31d7988294aa 144
bell_huang 0:31d7988294aa 145 int16_t heart_rate::gravity_to_int16(gravity_mode gravity, float value)
bell_huang 0:31d7988294aa 146 {
bell_huang 0:31d7988294aa 147 typedef int16_t result_type;
bell_huang 0:31d7988294aa 148 static const result_type MAX = std::numeric_limits<result_type>::max();
bell_huang 0:31d7988294aa 149 static const result_type MIN = std::numeric_limits<result_type>::min();
bell_huang 0:31d7988294aa 150
bell_huang 0:31d7988294aa 151 float result = (value * MAX) / ((float)gravity * GRAVITY_OF_EARTH);
bell_huang 0:31d7988294aa 152 if (result > MAX)
bell_huang 0:31d7988294aa 153 return MAX;
bell_huang 0:31d7988294aa 154 else if (result < MIN)
bell_huang 0:31d7988294aa 155 return MIN;
bell_huang 0:31d7988294aa 156 return (result_type)result;
bell_huang 0:31d7988294aa 157 }
bell_huang 0:31d7988294aa 158
bell_huang 0:31d7988294aa 159 void heart_rate::flush()
bell_huang 0:31d7988294aa 160 {
bell_huang 0:31d7988294aa 161 // Get sync timestamp
bell_huang 0:31d7988294aa 162 if (m_ppg_sample_queue.size() < PPG_SYNC_COUNT)
bell_huang 0:31d7988294aa 163 return;
bell_huang 0:31d7988294aa 164 uint64_t sync_timestamp = m_ppg_sample_queue[PPG_SYNC_COUNT - 1].timestamp;
bell_huang 0:31d7988294aa 165
bell_huang 0:31d7988294aa 166 // Ensure accel sensor has data timestamp later than sync_timestamp
bell_huang 0:31d7988294aa 167 if (m_ppg_sample_queue.size() < PPG_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 168 {
bell_huang 0:31d7988294aa 169 if (m_accel_sample_queue.empty() || (m_accel_sample_queue.back().timestamp < sync_timestamp))
bell_huang 0:31d7988294aa 170 return;
bell_huang 0:31d7988294aa 171 }
bell_huang 0:31d7988294aa 172
bell_huang 0:31d7988294aa 173 // Flush PPG
bell_huang 0:31d7988294aa 174 size_t flush_ppg_sample_count = PPG_SYNC_COUNT;
bell_huang 0:31d7988294aa 175 std::vector<int32_t> hr_ppg_data(flush_ppg_sample_count * PPG_CH_NUM);
bell_huang 0:31d7988294aa 176 for (size_t i = 0; i < flush_ppg_sample_count; ++i)
bell_huang 0:31d7988294aa 177 {
bell_huang 0:31d7988294aa 178 hr_ppg_data[i * PPG_CH_NUM + 0] = m_ppg_sample_queue[i].ch1;
bell_huang 0:31d7988294aa 179 hr_ppg_data[i * PPG_CH_NUM + 1] = m_ppg_sample_queue[i].ch2;
bell_huang 0:31d7988294aa 180 }
bell_huang 0:31d7988294aa 181 uint8_t touch_flag = m_ppg_sample_queue[flush_ppg_sample_count - 1].is_touched;
bell_huang 0:31d7988294aa 182 uint64_t ppg_timestamp = m_ppg_sample_queue[flush_ppg_sample_count - 1].timestamp;
bell_huang 0:31d7988294aa 183 if (m_last_report_timestamp == 0)
bell_huang 0:31d7988294aa 184 m_last_report_timestamp = m_ppg_sample_queue.front().timestamp;
bell_huang 0:31d7988294aa 185 std::deque<ppg_sample>::iterator flush_ppg_end_iter(m_ppg_sample_queue.begin());
bell_huang 0:31d7988294aa 186 std::advance(flush_ppg_end_iter, flush_ppg_sample_count);
bell_huang 0:31d7988294aa 187 m_ppg_sample_queue.erase(m_ppg_sample_queue.begin(), flush_ppg_end_iter);
bell_huang 0:31d7988294aa 188
bell_huang 0:31d7988294aa 189 // Flush ACCEL
bell_huang 0:31d7988294aa 190 size_t flush_accel_sample_count = 0;
bell_huang 0:31d7988294aa 191 for (size_t i = 0; i < m_accel_sample_queue.size(); ++i)
bell_huang 0:31d7988294aa 192 {
bell_huang 0:31d7988294aa 193 if (m_accel_sample_queue[i].timestamp <= sync_timestamp)
bell_huang 0:31d7988294aa 194 ++flush_accel_sample_count;
bell_huang 0:31d7988294aa 195 else
bell_huang 0:31d7988294aa 196 break;
bell_huang 0:31d7988294aa 197 }
bell_huang 0:31d7988294aa 198 std::vector<int16_t> hr_accel_data(flush_accel_sample_count * ACCEL_AXIS_NUM);
bell_huang 0:31d7988294aa 199 for (size_t i = 0; i < flush_accel_sample_count; ++i)
bell_huang 0:31d7988294aa 200 {
bell_huang 0:31d7988294aa 201 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 202 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 203 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 204 }
bell_huang 0:31d7988294aa 205 std::deque<accel_sample>::iterator flush_accel_end_iter(m_accel_sample_queue.begin());
bell_huang 0:31d7988294aa 206 std::advance(flush_accel_end_iter, flush_accel_sample_count);
bell_huang 0:31d7988294aa 207 m_accel_sample_queue.erase(m_accel_sample_queue.begin(), flush_accel_end_iter);
bell_huang 0:31d7988294aa 208
bell_huang 0:31d7988294aa 209 // Duration
bell_huang 0:31d7988294aa 210 uint32_t time_ms = (uint32_t)m_time_to_millisec_handler(ppg_timestamp - m_last_report_timestamp);
bell_huang 0:31d7988294aa 211 m_last_report_timestamp = ppg_timestamp;
bell_huang 0:31d7988294aa 212
bell_huang 1:e9f4fb198b63 213 pah_hr_data pah8series_data;
bell_huang 0:31d7988294aa 214 pah8series_data.frame_count = m_frame_count;
bell_huang 0:31d7988294aa 215 pah8series_data.time = time_ms;
bell_huang 0:31d7988294aa 216 pah8series_data.touch_flag = touch_flag;
bell_huang 0:31d7988294aa 217 pah8series_data.nf_ppg_channel = PPG_CH_NUM;
bell_huang 0:31d7988294aa 218 pah8series_data.nf_ppg_per_channel = flush_ppg_sample_count;
bell_huang 0:31d7988294aa 219 pah8series_data.ppg_data = &hr_ppg_data[0];
bell_huang 0:31d7988294aa 220 pah8series_data.nf_mems = flush_accel_sample_count;
bell_huang 0:31d7988294aa 221 pah8series_data.mems_data = &hr_accel_data[0];
bell_huang 0:31d7988294aa 222 ++m_frame_count;
bell_huang 0:31d7988294aa 223
bell_huang 2:5cb0e405a24c 224 if (m_debug_print_handler)
bell_huang 1:e9f4fb198b63 225 log_pah8series_data(&pah8series_data);
bell_huang 1:e9f4fb198b63 226
bell_huang 0:31d7988294aa 227 uint8_t ret = pah8series_entrance(&pah8series_data);
bell_huang 0:31d7988294aa 228
bell_huang 0:31d7988294aa 229 float hr = 0.0f;
bell_huang 0:31d7988294aa 230 pah8series_get_hr(&hr);
bell_huang 0:31d7988294aa 231
bell_huang 0:31d7988294aa 232 int hr_trust_level = 0;
bell_huang 0:31d7988294aa 233 pah8series_get_hr_trust_level(&hr_trust_level);
bell_huang 0:31d7988294aa 234
bell_huang 3:6d8153f8c92e 235 debug_printf("ret = %d, hr = %d, hr_trust_level = %d\n", ret, (int)hr, hr_trust_level);
bell_huang 3:6d8153f8c92e 236
bell_huang 0:31d7988294aa 237 if (ret == MSG_SUCCESS
bell_huang 0:31d7988294aa 238 || ret == MSG_HR_READY)
bell_huang 0:31d7988294aa 239 m_result.ret = ret_success;
bell_huang 0:31d7988294aa 240 else if (ret == MSG_NO_TOUCH)
bell_huang 0:31d7988294aa 241 m_result.ret = ret_no_touch;
bell_huang 0:31d7988294aa 242 else if (ret == MSG_SIGNAL_POOR)
bell_huang 0:31d7988294aa 243 m_result.ret = ret_signal_poor;
bell_huang 0:31d7988294aa 244 else
bell_huang 0:31d7988294aa 245 m_result.ret = ret_fail;
bell_huang 0:31d7988294aa 246 m_result.hr = hr;
bell_huang 0:31d7988294aa 247 m_result.hr_trust_level = hr_trust_level;
bell_huang 0:31d7988294aa 248 }
bell_huang 1:e9f4fb198b63 249
bell_huang 4:90dfc6d7461b 250 void heart_rate::log_pah8series_data_header(uint32_t ch_num, uint32_t ir_ch_num, uint32_t g_sensor_mode)
bell_huang 4:90dfc6d7461b 251 {
bell_huang 4:90dfc6d7461b 252 //log pah8series data header
bell_huang 4:90dfc6d7461b 253 // (1)Using total channel numbers;
bell_huang 4:90dfc6d7461b 254 // (2)reserved;
bell_huang 4:90dfc6d7461b 255 // (3)reserved;
bell_huang 4:90dfc6d7461b 256 // (4)IR channel number;
bell_huang 4:90dfc6d7461b 257 // (5)MEMS mode 0:2G, 1:4G, 2:8G
bell_huang 4:90dfc6d7461b 258 log_printf("PPG CH#, %d, %d, %d, %d, %d\n", ch_num, 0, 0, ir_ch_num, g_sensor_mode);
bell_huang 4:90dfc6d7461b 259 }
bell_huang 4:90dfc6d7461b 260
bell_huang 1:e9f4fb198b63 261 void heart_rate::log_pah8series_data(const pah_hr_data *pxialg_data)
bell_huang 1:e9f4fb198b63 262 {
bell_huang 1:e9f4fb198b63 263 int i = 0;
bell_huang 1:e9f4fb198b63 264 uint32_t *ppg_data = (uint32_t *)pxialg_data->ppg_data;
bell_huang 1:e9f4fb198b63 265 int16_t *mems_data = pxialg_data->mems_data;
bell_huang 1:e9f4fb198b63 266 int data_num = pxialg_data->nf_ppg_channel * pxialg_data->nf_ppg_per_channel;
bell_huang 1:e9f4fb198b63 267
bell_huang 1:e9f4fb198b63 268 log_printf("Frame Count, %d \n", pxialg_data->frame_count);
bell_huang 1:e9f4fb198b63 269 log_printf("Time, %d \n", pxialg_data->time);
bell_huang 1:e9f4fb198b63 270 log_printf("PPG, %d, %d, ", pxialg_data->touch_flag, data_num);
bell_huang 1:e9f4fb198b63 271 for (i = 0; i < data_num; ++i)
bell_huang 1:e9f4fb198b63 272 {
bell_huang 1:e9f4fb198b63 273 log_printf("%d, ", *ppg_data);
bell_huang 1:e9f4fb198b63 274 ppg_data++;
bell_huang 1:e9f4fb198b63 275 }
bell_huang 1:e9f4fb198b63 276 log_printf("\n");
bell_huang 1:e9f4fb198b63 277 log_printf("MEMS, %d, ", pxialg_data->nf_mems);
bell_huang 1:e9f4fb198b63 278 for (i = 0; i < pxialg_data->nf_mems * 3; ++i)
bell_huang 1:e9f4fb198b63 279 {
bell_huang 1:e9f4fb198b63 280 log_printf("%d, ", *mems_data);
bell_huang 1:e9f4fb198b63 281 mems_data++;
bell_huang 1:e9f4fb198b63 282 }
bell_huang 1:e9f4fb198b63 283 log_printf("\n");
bell_huang 1:e9f4fb198b63 284 }
bell_huang 1:e9f4fb198b63 285
bell_huang 0:31d7988294aa 286
bell_huang 0:31d7988294aa 287 }
bell_huang 0:31d7988294aa 288