Bell Huang / pixart_heart_rate

class heart_rate

pixart_heart_rate.cpp@2:5cb0e405a24c, 2017-10-27 (annotated)

Committer:
bell_huang
Date:
Fri Oct 27 06:39:17 2017 +0000
Revision:
2:5cb0e405a24c
Parent:
1:e9f4fb198b63
Child:
3:6d8153f8c92e
QQQQ

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