Bell Huang / pixart_heart_rate

class heart_rate

pixart_heart_rate.cpp@3:6d8153f8c92e, 2017-10-27 (annotated)

Committer:
bell_huang
Date:
Fri Oct 27 06:45:49 2017 +0000
Revision:
3:6d8153f8c92e
Parent:
2:5cb0e405a24c
Child:
4:90dfc6d7461b
abc123

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 0:31d7988294aa 68 m_gravity_mode = gravity;
bell_huang 0:31d7988294aa 69
bell_huang 0:31d7988294aa 70 return true;
bell_huang 0:31d7988294aa 71 }
bell_huang 0:31d7988294aa 72
bell_huang 2:5cb0e405a24c 73 void heart_rate::enable_debug_print(DEBUG_PRINT_HANDLE handler)
bell_huang 1:e9f4fb198b63 74 {
bell_huang 2:5cb0e405a24c 75 m_debug_print_handler = handler;
bell_huang 1:e9f4fb198b63 76 }
bell_huang 1:e9f4fb198b63 77
bell_huang 0:31d7988294aa 78 void heart_rate::reset()
bell_huang 0:31d7988294aa 79 {
bell_huang 0:31d7988294aa 80 m_ppg_sample_queue.clear();
bell_huang 0:31d7988294aa 81 m_accel_sample_queue.clear();
bell_huang 0:31d7988294aa 82 m_last_report_timestamp = 0;
bell_huang 0:31d7988294aa 83 m_frame_count = 0;
bell_huang 0:31d7988294aa 84 memset(&m_result, 0, sizeof(m_result));
bell_huang 0:31d7988294aa 85 }
bell_huang 0:31d7988294aa 86
bell_huang 0:31d7988294aa 87 bool heart_rate::set_ppg_sample(const ppg_sample &ppg_sample)
bell_huang 0:31d7988294aa 88 {
bell_huang 0:31d7988294aa 89 // overflow
bell_huang 0:31d7988294aa 90 if (m_ppg_sample_queue.size() + 1 > PPG_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 91 return false;
bell_huang 0:31d7988294aa 92
bell_huang 0:31d7988294aa 93 m_ppg_sample_queue.push_back(ppg_sample);
bell_huang 0:31d7988294aa 94
bell_huang 0:31d7988294aa 95 flush();
bell_huang 0:31d7988294aa 96
bell_huang 0:31d7988294aa 97 return true;
bell_huang 0:31d7988294aa 98 }
bell_huang 0:31d7988294aa 99
bell_huang 0:31d7988294aa 100 bool heart_rate::set_accel_sample(const accel_sample &accel_sample)
bell_huang 0:31d7988294aa 101 {
bell_huang 0:31d7988294aa 102 // overflow
bell_huang 0:31d7988294aa 103 if (m_accel_sample_queue.size() + 1 > ACCEL_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 104 return false;
bell_huang 0:31d7988294aa 105
bell_huang 0:31d7988294aa 106 m_accel_sample_queue.push_back(accel_sample);
bell_huang 0:31d7988294aa 107
bell_huang 0:31d7988294aa 108 return true;
bell_huang 0:31d7988294aa 109 }
bell_huang 0:31d7988294aa 110
bell_huang 0:31d7988294aa 111 const heart_rate::result& heart_rate::get_result() const
bell_huang 0:31d7988294aa 112 {
bell_huang 0:31d7988294aa 113 return m_result;
bell_huang 0:31d7988294aa 114 }
bell_huang 0:31d7988294aa 115
bell_huang 0:31d7988294aa 116 int heart_rate::gravity_to_hr_alg_gsensor_mode(gravity_mode gravity)
bell_huang 0:31d7988294aa 117 {
bell_huang 0:31d7988294aa 118 int gsensor_mode = 0;
bell_huang 0:31d7988294aa 119
bell_huang 0:31d7988294aa 120 switch (gravity)
bell_huang 0:31d7988294aa 121 {
bell_huang 0:31d7988294aa 122 case gravity_2g:
bell_huang 0:31d7988294aa 123 gsensor_mode = 0;
bell_huang 0:31d7988294aa 124 break;
bell_huang 0:31d7988294aa 125
bell_huang 0:31d7988294aa 126 case gravity_4g:
bell_huang 0:31d7988294aa 127 gsensor_mode = 1;
bell_huang 0:31d7988294aa 128 break;
bell_huang 0:31d7988294aa 129
bell_huang 0:31d7988294aa 130 case gravity_8g:
bell_huang 0:31d7988294aa 131 gsensor_mode = 2;
bell_huang 0:31d7988294aa 132 break;
bell_huang 0:31d7988294aa 133
bell_huang 0:31d7988294aa 134 case gravity_16g:
bell_huang 0:31d7988294aa 135 default:
bell_huang 0:31d7988294aa 136 gsensor_mode = 3;
bell_huang 0:31d7988294aa 137 break;
bell_huang 0:31d7988294aa 138 }
bell_huang 0:31d7988294aa 139
bell_huang 0:31d7988294aa 140 return gsensor_mode;
bell_huang 0:31d7988294aa 141 }
bell_huang 0:31d7988294aa 142
bell_huang 0:31d7988294aa 143 int16_t heart_rate::gravity_to_int16(gravity_mode gravity, float value)
bell_huang 0:31d7988294aa 144 {
bell_huang 0:31d7988294aa 145 typedef int16_t result_type;
bell_huang 0:31d7988294aa 146 static const result_type MAX = std::numeric_limits<result_type>::max();
bell_huang 0:31d7988294aa 147 static const result_type MIN = std::numeric_limits<result_type>::min();
bell_huang 0:31d7988294aa 148
bell_huang 0:31d7988294aa 149 float result = (value * MAX) / ((float)gravity * GRAVITY_OF_EARTH);
bell_huang 0:31d7988294aa 150 if (result > MAX)
bell_huang 0:31d7988294aa 151 return MAX;
bell_huang 0:31d7988294aa 152 else if (result < MIN)
bell_huang 0:31d7988294aa 153 return MIN;
bell_huang 0:31d7988294aa 154 return (result_type)result;
bell_huang 0:31d7988294aa 155 }
bell_huang 0:31d7988294aa 156
bell_huang 0:31d7988294aa 157 void heart_rate::flush()
bell_huang 0:31d7988294aa 158 {
bell_huang 0:31d7988294aa 159 // Get sync timestamp
bell_huang 0:31d7988294aa 160 if (m_ppg_sample_queue.size() < PPG_SYNC_COUNT)
bell_huang 0:31d7988294aa 161 return;
bell_huang 0:31d7988294aa 162 uint64_t sync_timestamp = m_ppg_sample_queue[PPG_SYNC_COUNT - 1].timestamp;
bell_huang 0:31d7988294aa 163
bell_huang 0:31d7988294aa 164 // Ensure accel sensor has data timestamp later than sync_timestamp
bell_huang 0:31d7988294aa 165 if (m_ppg_sample_queue.size() < PPG_MAX_SAMPLE_NUM)
bell_huang 0:31d7988294aa 166 {
bell_huang 0:31d7988294aa 167 if (m_accel_sample_queue.empty() || (m_accel_sample_queue.back().timestamp < sync_timestamp))
bell_huang 0:31d7988294aa 168 return;
bell_huang 0:31d7988294aa 169 }
bell_huang 0:31d7988294aa 170
bell_huang 0:31d7988294aa 171 // Flush PPG
bell_huang 0:31d7988294aa 172 size_t flush_ppg_sample_count = PPG_SYNC_COUNT;
bell_huang 0:31d7988294aa 173 std::vector<int32_t> hr_ppg_data(flush_ppg_sample_count * PPG_CH_NUM);
bell_huang 0:31d7988294aa 174 for (size_t i = 0; i < flush_ppg_sample_count; ++i)
bell_huang 0:31d7988294aa 175 {
bell_huang 0:31d7988294aa 176 hr_ppg_data[i * PPG_CH_NUM + 0] = m_ppg_sample_queue[i].ch1;
bell_huang 0:31d7988294aa 177 hr_ppg_data[i * PPG_CH_NUM + 1] = m_ppg_sample_queue[i].ch2;
bell_huang 0:31d7988294aa 178 }
bell_huang 0:31d7988294aa 179 uint8_t touch_flag = m_ppg_sample_queue[flush_ppg_sample_count - 1].is_touched;
bell_huang 0:31d7988294aa 180 uint64_t ppg_timestamp = m_ppg_sample_queue[flush_ppg_sample_count - 1].timestamp;
bell_huang 0:31d7988294aa 181 if (m_last_report_timestamp == 0)
bell_huang 0:31d7988294aa 182 m_last_report_timestamp = m_ppg_sample_queue.front().timestamp;
bell_huang 0:31d7988294aa 183 std::deque<ppg_sample>::iterator flush_ppg_end_iter(m_ppg_sample_queue.begin());
bell_huang 0:31d7988294aa 184 std::advance(flush_ppg_end_iter, flush_ppg_sample_count);
bell_huang 0:31d7988294aa 185 m_ppg_sample_queue.erase(m_ppg_sample_queue.begin(), flush_ppg_end_iter);
bell_huang 0:31d7988294aa 186
bell_huang 0:31d7988294aa 187 // Flush ACCEL
bell_huang 0:31d7988294aa 188 size_t flush_accel_sample_count = 0;
bell_huang 0:31d7988294aa 189 for (size_t i = 0; i < m_accel_sample_queue.size(); ++i)
bell_huang 0:31d7988294aa 190 {
bell_huang 0:31d7988294aa 191 if (m_accel_sample_queue[i].timestamp <= sync_timestamp)
bell_huang 0:31d7988294aa 192 ++flush_accel_sample_count;
bell_huang 0:31d7988294aa 193 else
bell_huang 0:31d7988294aa 194 break;
bell_huang 0:31d7988294aa 195 }
bell_huang 0:31d7988294aa 196 std::vector<int16_t> hr_accel_data(flush_accel_sample_count * ACCEL_AXIS_NUM);
bell_huang 0:31d7988294aa 197 for (size_t i = 0; i < flush_accel_sample_count; ++i)
bell_huang 0:31d7988294aa 198 {
bell_huang 0:31d7988294aa 199 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 200 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 201 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 202 }
bell_huang 0:31d7988294aa 203 std::deque<accel_sample>::iterator flush_accel_end_iter(m_accel_sample_queue.begin());
bell_huang 0:31d7988294aa 204 std::advance(flush_accel_end_iter, flush_accel_sample_count);
bell_huang 0:31d7988294aa 205 m_accel_sample_queue.erase(m_accel_sample_queue.begin(), flush_accel_end_iter);
bell_huang 0:31d7988294aa 206
bell_huang 0:31d7988294aa 207 // Duration
bell_huang 0:31d7988294aa 208 uint32_t time_ms = (uint32_t)m_time_to_millisec_handler(ppg_timestamp - m_last_report_timestamp);
bell_huang 0:31d7988294aa 209 m_last_report_timestamp = ppg_timestamp;
bell_huang 0:31d7988294aa 210
bell_huang 1:e9f4fb198b63 211 pah_hr_data pah8series_data;
bell_huang 0:31d7988294aa 212 pah8series_data.frame_count = m_frame_count;
bell_huang 0:31d7988294aa 213 pah8series_data.time = time_ms;
bell_huang 0:31d7988294aa 214 pah8series_data.touch_flag = touch_flag;
bell_huang 0:31d7988294aa 215 pah8series_data.nf_ppg_channel = PPG_CH_NUM;
bell_huang 0:31d7988294aa 216 pah8series_data.nf_ppg_per_channel = flush_ppg_sample_count;
bell_huang 0:31d7988294aa 217 pah8series_data.ppg_data = &hr_ppg_data[0];
bell_huang 0:31d7988294aa 218 pah8series_data.nf_mems = flush_accel_sample_count;
bell_huang 0:31d7988294aa 219 pah8series_data.mems_data = &hr_accel_data[0];
bell_huang 0:31d7988294aa 220 ++m_frame_count;
bell_huang 0:31d7988294aa 221
bell_huang 2:5cb0e405a24c 222 if (m_debug_print_handler)
bell_huang 1:e9f4fb198b63 223 log_pah8series_data(&pah8series_data);
bell_huang 1:e9f4fb198b63 224
bell_huang 0:31d7988294aa 225 uint8_t ret = pah8series_entrance(&pah8series_data);
bell_huang 0:31d7988294aa 226
bell_huang 0:31d7988294aa 227 float hr = 0.0f;
bell_huang 0:31d7988294aa 228 pah8series_get_hr(&hr);
bell_huang 0:31d7988294aa 229
bell_huang 0:31d7988294aa 230 int hr_trust_level = 0;
bell_huang 0:31d7988294aa 231 pah8series_get_hr_trust_level(&hr_trust_level);
bell_huang 0:31d7988294aa 232
bell_huang 3:6d8153f8c92e 233 debug_printf("ret = %d, hr = %d, hr_trust_level = %d\n", ret, (int)hr, hr_trust_level);
bell_huang 3:6d8153f8c92e 234
bell_huang 0:31d7988294aa 235 if (ret == MSG_SUCCESS
bell_huang 0:31d7988294aa 236 || ret == MSG_HR_READY)
bell_huang 0:31d7988294aa 237 m_result.ret = ret_success;
bell_huang 0:31d7988294aa 238 else if (ret == MSG_NO_TOUCH)
bell_huang 0:31d7988294aa 239 m_result.ret = ret_no_touch;
bell_huang 0:31d7988294aa 240 else if (ret == MSG_SIGNAL_POOR)
bell_huang 0:31d7988294aa 241 m_result.ret = ret_signal_poor;
bell_huang 0:31d7988294aa 242 else
bell_huang 0:31d7988294aa 243 m_result.ret = ret_fail;
bell_huang 0:31d7988294aa 244 m_result.hr = hr;
bell_huang 0:31d7988294aa 245 m_result.hr_trust_level = hr_trust_level;
bell_huang 0:31d7988294aa 246 }
bell_huang 1:e9f4fb198b63 247
bell_huang 1:e9f4fb198b63 248 void heart_rate::log_pah8series_data(const pah_hr_data *pxialg_data)
bell_huang 1:e9f4fb198b63 249 {
bell_huang 1:e9f4fb198b63 250 int i = 0;
bell_huang 1:e9f4fb198b63 251 uint32_t *ppg_data = (uint32_t *)pxialg_data->ppg_data;
bell_huang 1:e9f4fb198b63 252 int16_t *mems_data = pxialg_data->mems_data;
bell_huang 1:e9f4fb198b63 253 int data_num = pxialg_data->nf_ppg_channel * pxialg_data->nf_ppg_per_channel;
bell_huang 1:e9f4fb198b63 254
bell_huang 1:e9f4fb198b63 255 log_printf("Frame Count, %d \n", pxialg_data->frame_count);
bell_huang 1:e9f4fb198b63 256 log_printf("Time, %d \n", pxialg_data->time);
bell_huang 1:e9f4fb198b63 257 log_printf("PPG, %d, %d, ", pxialg_data->touch_flag, data_num);
bell_huang 1:e9f4fb198b63 258 for (i = 0; i < data_num; ++i)
bell_huang 1:e9f4fb198b63 259 {
bell_huang 1:e9f4fb198b63 260 log_printf("%d, ", *ppg_data);
bell_huang 1:e9f4fb198b63 261 ppg_data++;
bell_huang 1:e9f4fb198b63 262 }
bell_huang 1:e9f4fb198b63 263 log_printf("\n");
bell_huang 1:e9f4fb198b63 264 log_printf("MEMS, %d, ", pxialg_data->nf_mems);
bell_huang 1:e9f4fb198b63 265 for (i = 0; i < pxialg_data->nf_mems * 3; ++i)
bell_huang 1:e9f4fb198b63 266 {
bell_huang 1:e9f4fb198b63 267 log_printf("%d, ", *mems_data);
bell_huang 1:e9f4fb198b63 268 mems_data++;
bell_huang 1:e9f4fb198b63 269 }
bell_huang 1:e9f4fb198b63 270 log_printf("\n");
bell_huang 1:e9f4fb198b63 271 }
bell_huang 1:e9f4fb198b63 272
bell_huang 0:31d7988294aa 273
bell_huang 0:31d7988294aa 274 }
bell_huang 0:31d7988294aa 275