Chanel's edits
Dependencies: max32630fthr USBDevice
bt32630.h@19:f230229cb6f3, 2020-04-07 (annotated)
- Committer:
- saleiferis
- Date:
- Tue Apr 07 04:57:22 2020 +0000
- Revision:
- 19:f230229cb6f3
- Parent:
- 17:65a8b29f6bac
HeartRateService
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
saleiferis | 19:f230229cb6f3 | 1 | #include "HeartRateService.h" |
saleiferis | 7:4debec043316 | 2 | #include "ECGService.h" |
saleiferis | 14:ee2175578993 | 3 | #include "PPGService.h" |
saleiferis | 14:ee2175578993 | 4 | #include "filters.h" |
saleiferis | 16:f6bfa6b66e96 | 5 | #include "panTompkins.h" |
saleiferis | 14:ee2175578993 | 6 | |
saleiferis | 14:ee2175578993 | 7 | //Register definitions |
saleiferis | 14:ee2175578993 | 8 | #define MAX86150_Addr 0xBC //updated per I2Cscanner, 8 bit version of 7 bit code 0x5E |
saleiferis | 14:ee2175578993 | 9 | #define maxi2cFreq 1000000 |
saleiferis | 14:ee2175578993 | 10 | #define recommendedi2cFreq 400000 |
saleiferis | 14:ee2175578993 | 11 | |
saleiferis | 14:ee2175578993 | 12 | #define BaudRate 115200 |
saleiferis | 14:ee2175578993 | 13 | |
saleiferis | 14:ee2175578993 | 14 | #define WINDOW_SIZE 25 // number of samples for moving winfow integration for HR algo preprocessing. Effective window_size is half of this value |
saleiferis | 14:ee2175578993 | 15 | #define BUFF_SIZE 136 // TODO: decouple BUFF_SIZE and FIR_SIZE. now program crashes if != 136 |
saleiferis | 14:ee2175578993 | 16 | #define FIR_SIZE 136 |
saleiferis | 7:4debec043316 | 17 | |
saleiferis | 1:6e6f7e3cc1e1 | 18 | extern MAX86150 max86150Sensor; |
saleiferis | 1:6e6f7e3cc1e1 | 19 | extern Serial pc; |
saleiferis | 14:ee2175578993 | 20 | extern I2C i2c; |
saleiferis | 14:ee2175578993 | 21 | extern BLE &ble; |
saleiferis | 15:b15b4b6c6da8 | 22 | const static char DEVICE_NAME[] = "MAX32630"; |
saleiferis | 1:6e6f7e3cc1e1 | 23 | static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE}; |
saleiferis | 1:6e6f7e3cc1e1 | 24 | |
saleiferis | 14:ee2175578993 | 25 | // |
saleiferis | 14:ee2175578993 | 26 | extern int k; // loop iteration |
saleiferis | 14:ee2175578993 | 27 | extern float signal[BUFF_SIZE]; //store signal segment to be filtered |
saleiferis | 14:ee2175578993 | 28 | extern float bp_signal[BUFF_SIZE]; |
saleiferis | 14:ee2175578993 | 29 | extern float derivative[BUFF_SIZE]; |
saleiferis | 14:ee2175578993 | 30 | extern float squared[BUFF_SIZE]; |
saleiferis | 14:ee2175578993 | 31 | extern float integral[BUFF_SIZE]; |
saleiferis | 16:f6bfa6b66e96 | 32 | extern bool outputSignal[BUFF_SIZE]; |
saleiferis | 14:ee2175578993 | 33 | |
saleiferis | 14:ee2175578993 | 34 | // |
saleiferis | 14:ee2175578993 | 35 | extern int rr1[8], rr2[8], rravg1, rravg2, rrlow , rrhigh , rrmiss ; |
saleiferis | 16:f6bfa6b66e96 | 36 | extern long unsigned int i, j, sample , current , lastQRS , lastSlope , currentSlope ; |
saleiferis | 14:ee2175578993 | 37 | extern float peak_i , peak_f , threshold_i1 , threshold_i2 , threshold_f1 , threshold_f2 , spk_i , spk_f , npk_i , npk_f ; |
saleiferis | 14:ee2175578993 | 38 | extern bool qrs, regular, prevRegular; |
saleiferis | 14:ee2175578993 | 39 | |
saleiferis | 14:ee2175578993 | 40 | // Variables for preprocessing |
saleiferis | 14:ee2175578993 | 41 | extern float y ; //filtered sample to be displayed |
saleiferis | 14:ee2175578993 | 42 | extern float prev_y ; // keep track of previous output to calculate derivative |
saleiferis | 14:ee2175578993 | 43 | extern float sq_y; |
saleiferis | 14:ee2175578993 | 44 | extern float movmean; // result of moving window integration |
saleiferis | 14:ee2175578993 | 45 | |
saleiferis | 7:4debec043316 | 46 | |
saleiferis | 10:28b8729cf5dc | 47 | static int16_t hrmCounter = 100; // init HRM to 100bps |
saleiferis | 19:f230229cb6f3 | 48 | extern HeartRateService *hrServicePtr; |
saleiferis | 19:f230229cb6f3 | 49 | //extern ECGService *hrServicePtr; |
saleiferis | 1:6e6f7e3cc1e1 | 50 | |
saleiferis | 1:6e6f7e3cc1e1 | 51 | static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE); |
saleiferis | 1:6e6f7e3cc1e1 | 52 | |
saleiferis | 1:6e6f7e3cc1e1 | 53 | |
saleiferis | 4:4233f5538abf | 54 | void connectionCallback(const Gap::ConnectionCallbackParams_t *params) |
saleiferis | 4:4233f5538abf | 55 | { |
saleiferis | 4:4233f5538abf | 56 | pc.printf("Connected to BLE Client...\n"); |
saleiferis | 14:ee2175578993 | 57 | max86150Sensor.begin(i2c, recommendedi2cFreq, MAX86150_Addr); |
saleiferis | 14:ee2175578993 | 58 | wait_ms(300); |
saleiferis | 14:ee2175578993 | 59 | |
saleiferis | 14:ee2175578993 | 60 | //unsigned char partID = max86150Sensor.readPartID(); |
saleiferis | 14:ee2175578993 | 61 | unsigned char partID = max86150Sensor.readRegister8(MAX86150_Addr,0xFF); |
saleiferis | 14:ee2175578993 | 62 | pc.printf("Part ID is: %X\n",partID); |
saleiferis | 14:ee2175578993 | 63 | while (partID != 0x1E) {/* Connection to sensor is not established */ } |
saleiferis | 14:ee2175578993 | 64 | |
saleiferis | 14:ee2175578993 | 65 | |
saleiferis | 14:ee2175578993 | 66 | //***** SETUP SENSOR */ |
saleiferis | 14:ee2175578993 | 67 | max86150Sensor.setup(); //Configure sensor |
saleiferis | 14:ee2175578993 | 68 | wait_ms(300); |
saleiferis | 14:ee2175578993 | 69 | pc.printf("SYSCONTOL REG: %x\n", max86150Sensor.readRegister8(MAX86150_Addr,0x0D)); |
saleiferis | 14:ee2175578993 | 70 | pc.printf("FIFO CONFIG: %X\n",max86150Sensor.readRegister8(MAX86150_Addr,0x08)); |
saleiferis | 14:ee2175578993 | 71 | pc.printf("INT_EN1: %X\n", max86150Sensor.readRegister8(MAX86150_Addr,0x02)); |
saleiferis | 14:ee2175578993 | 72 | pc.printf("INT_EN2: %X\n", max86150Sensor.readRegister8(MAX86150_Addr,0x03)); |
saleiferis | 14:ee2175578993 | 73 | pc.printf("INT STATUS1: %X\n",max86150Sensor.readRegister8(MAX86150_Addr,0x00)); |
saleiferis | 14:ee2175578993 | 74 | pc.printf("INT STATUS2: %X\n",max86150Sensor.readRegister8(MAX86150_Addr,0x01)); |
saleiferis | 14:ee2175578993 | 75 | //*************************************************************// |
saleiferis | 16:f6bfa6b66e96 | 76 | panTompkinsInit(); |
saleiferis | 14:ee2175578993 | 77 | max86150Sensor.clearFIFO(); |
saleiferis | 14:ee2175578993 | 78 | max86150Sensor.writeRegister8(MAX86150_Addr,0x0D,0x04); //start FIFO |
saleiferis | 14:ee2175578993 | 79 | |
saleiferis | 4:4233f5538abf | 80 | } |
saleiferis | 4:4233f5538abf | 81 | |
saleiferis | 1:6e6f7e3cc1e1 | 82 | void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params) |
saleiferis | 1:6e6f7e3cc1e1 | 83 | { |
saleiferis | 1:6e6f7e3cc1e1 | 84 | BLE::Instance().gap().startAdvertising(); // restart advertising |
saleiferis | 1:6e6f7e3cc1e1 | 85 | } |
saleiferis | 1:6e6f7e3cc1e1 | 86 | |
saleiferis | 1:6e6f7e3cc1e1 | 87 | void updateSensorValue() { |
saleiferis | 1:6e6f7e3cc1e1 | 88 | // Do blocking calls or whatever is necessary for sensor polling. |
saleiferis | 1:6e6f7e3cc1e1 | 89 | // In our case, we simply update the HRM measurement. |
saleiferis | 14:ee2175578993 | 90 | int16_t ecgsigned16; |
saleiferis | 14:ee2175578993 | 91 | uint32_t ppgRed16; |
saleiferis | 14:ee2175578993 | 92 | if(max86150Sensor.check()>0){ |
saleiferis | 14:ee2175578993 | 93 | // if buffer is full |
saleiferis | 16:f6bfa6b66e96 | 94 | if (sample >= BUFF_SIZE){ |
saleiferis | 14:ee2175578993 | 95 | for (k=0; k<BUFF_SIZE-1; k++){ //discard oldest sample, shift samples in buffer |
saleiferis | 14:ee2175578993 | 96 | signal[k] = signal[k+1]; |
saleiferis | 14:ee2175578993 | 97 | bp_signal[k] = bp_signal[k+1]; |
saleiferis | 14:ee2175578993 | 98 | derivative[k] = derivative[k+1]; |
saleiferis | 14:ee2175578993 | 99 | integral[k] = integral[k+1]; |
saleiferis | 14:ee2175578993 | 100 | squared[k] = squared[k+1]; |
saleiferis | 14:ee2175578993 | 101 | |
saleiferis | 14:ee2175578993 | 102 | } |
saleiferis | 16:f6bfa6b66e96 | 103 | current = BUFF_SIZE-1; // indicates that buffer is full |
saleiferis | 14:ee2175578993 | 104 | prev_y = y; |
saleiferis | 14:ee2175578993 | 105 | y = 0.0; // reset filter output for current sample |
saleiferis | 14:ee2175578993 | 106 | }else{ //Buffer is not full yet |
saleiferis | 16:f6bfa6b66e96 | 107 | current = sample; //position at buffer is same as sample number |
saleiferis | 14:ee2175578993 | 108 | } |
saleiferis | 14:ee2175578993 | 109 | ecgsigned16 = (int16_t) (max86150Sensor.getFIFOECG()>>2); //read an ECG sample |
saleiferis | 14:ee2175578993 | 110 | ppgRed16 = max86150Sensor.getFIFORed(); |
saleiferis | 14:ee2175578993 | 111 | max86150Sensor.nextSample(); // advance tail to get sample in next iteration |
saleiferis | 16:f6bfa6b66e96 | 112 | signal[current] = (float)ecgsigned16; //add sample to buffer |
saleiferis | 16:f6bfa6b66e96 | 113 | sample++; |
saleiferis | 14:ee2175578993 | 114 | |
saleiferis | 16:f6bfa6b66e96 | 115 | if (current< FIR_SIZE-1){ |
saleiferis | 14:ee2175578993 | 116 | // buffer is not full yet |
saleiferis | 14:ee2175578993 | 117 | }else{ //buffer is full, filter current sample |
saleiferis | 14:ee2175578993 | 118 | for(k = 0; k < FIR_SIZE; k++){ //FIR bandpass filter |
saleiferis | 16:f6bfa6b66e96 | 119 | y = y + FIR_BP[k] * signal[current-k]; |
saleiferis | 14:ee2175578993 | 120 | } |
saleiferis | 16:f6bfa6b66e96 | 121 | bp_signal[current] = y; |
saleiferis | 14:ee2175578993 | 122 | } |
saleiferis | 14:ee2175578993 | 123 | sq_y = pow(y-prev_y,2); |
saleiferis | 16:f6bfa6b66e96 | 124 | squared[current] = sq_y; |
saleiferis | 14:ee2175578993 | 125 | //moving window integration |
saleiferis | 14:ee2175578993 | 126 | movmean = 0.0; // reset for current sample |
saleiferis | 14:ee2175578993 | 127 | for(k=0; k<(int)WINDOW_SIZE/2; k++){ |
saleiferis | 16:f6bfa6b66e96 | 128 | //pc.printf("%d\n",(int)current - (int)(WINDOW_SIZE)); |
saleiferis | 16:f6bfa6b66e96 | 129 | if ((int)current - (int)(WINDOW_SIZE) > k){ //there are enough samples in squared[] |
saleiferis | 16:f6bfa6b66e96 | 130 | // movmean = movmean + squared[(current-k-(int)WINDOW_SIZE/2)] + squared[current+k-(int)WINDOW_SIZE/2]; |
saleiferis | 16:f6bfa6b66e96 | 131 | movmean = movmean + squared[current-k]; |
saleiferis | 14:ee2175578993 | 132 | |
saleiferis | 14:ee2175578993 | 133 | }else{ |
saleiferis | 14:ee2175578993 | 134 | break; |
saleiferis | 14:ee2175578993 | 135 | } |
saleiferis | 14:ee2175578993 | 136 | } |
saleiferis | 14:ee2175578993 | 137 | movmean = movmean/(float)(k+1); |
saleiferis | 16:f6bfa6b66e96 | 138 | integral[current] = movmean; |
saleiferis | 14:ee2175578993 | 139 | |
saleiferis | 16:f6bfa6b66e96 | 140 | // |
saleiferis | 14:ee2175578993 | 141 | //pc.printf("%d\n",ppgRed16); |
saleiferis | 14:ee2175578993 | 142 | |
saleiferis | 17:65a8b29f6bac | 143 | //hrServicePtr->updateHeartRate(ecgsigned16); //send ECG sample |
saleiferis | 14:ee2175578993 | 144 | // TODO: call pan_tompkins() |
saleiferis | 14:ee2175578993 | 145 | |
saleiferis | 16:f6bfa6b66e96 | 146 | panTompkinsIter(); |
saleiferis | 16:f6bfa6b66e96 | 147 | //pc.printf("%f %f %f \n",movmean/1000, y/50,(movmean/1000)*(float)outputSignal[current]); |
saleiferis | 16:f6bfa6b66e96 | 148 | int HR = (int)((FS*60)/rr1[7]); |
saleiferis | 17:65a8b29f6bac | 149 | int avg_HR = (int)((FS*60)/rravg1); |
saleiferis | 17:65a8b29f6bac | 150 | pc.printf("%f %f %d %d\n", y/50,(movmean/1000)*outputSignal[current],HR,avg_HR); |
saleiferis | 16:f6bfa6b66e96 | 151 | //pc.printf("%f f\n",y,y*(float)outputSignal[current]); |
saleiferis | 19:f230229cb6f3 | 152 | hrServicePtr->updateHeartRate((uint8_t)avg_HR); |
saleiferis | 16:f6bfa6b66e96 | 153 | |
saleiferis | 10:28b8729cf5dc | 154 | } |
saleiferis | 14:ee2175578993 | 155 | |
saleiferis | 10:28b8729cf5dc | 156 | |
saleiferis | 10:28b8729cf5dc | 157 | |
saleiferis | 1:6e6f7e3cc1e1 | 158 | } |
saleiferis | 1:6e6f7e3cc1e1 | 159 | |
saleiferis | 1:6e6f7e3cc1e1 | 160 | void periodicCallback(void) |
saleiferis | 1:6e6f7e3cc1e1 | 161 | { |
saleiferis | 1:6e6f7e3cc1e1 | 162 | |
saleiferis | 1:6e6f7e3cc1e1 | 163 | if (BLE::Instance().getGapState().connected) { |
saleiferis | 1:6e6f7e3cc1e1 | 164 | eventQueue.call(updateSensorValue); |
saleiferis | 1:6e6f7e3cc1e1 | 165 | } |
saleiferis | 1:6e6f7e3cc1e1 | 166 | } |
saleiferis | 1:6e6f7e3cc1e1 | 167 | |
saleiferis | 1:6e6f7e3cc1e1 | 168 | void onBleInitError(BLE &ble, ble_error_t error) |
saleiferis | 1:6e6f7e3cc1e1 | 169 | { |
saleiferis | 1:6e6f7e3cc1e1 | 170 | (void)ble; |
saleiferis | 1:6e6f7e3cc1e1 | 171 | (void)error; |
saleiferis | 1:6e6f7e3cc1e1 | 172 | /* Initialization error handling should go here */ |
saleiferis | 1:6e6f7e3cc1e1 | 173 | } |
saleiferis | 1:6e6f7e3cc1e1 | 174 | |
saleiferis | 1:6e6f7e3cc1e1 | 175 | void bleInitComplete(BLE::InitializationCompleteCallbackContext *params) |
saleiferis | 1:6e6f7e3cc1e1 | 176 | { |
saleiferis | 1:6e6f7e3cc1e1 | 177 | BLE& ble = params->ble; |
saleiferis | 1:6e6f7e3cc1e1 | 178 | ble_error_t error = params->error; |
saleiferis | 1:6e6f7e3cc1e1 | 179 | |
saleiferis | 1:6e6f7e3cc1e1 | 180 | if (error != BLE_ERROR_NONE) { |
saleiferis | 1:6e6f7e3cc1e1 | 181 | onBleInitError(ble, error); |
saleiferis | 1:6e6f7e3cc1e1 | 182 | return; |
saleiferis | 1:6e6f7e3cc1e1 | 183 | } |
saleiferis | 1:6e6f7e3cc1e1 | 184 | |
saleiferis | 1:6e6f7e3cc1e1 | 185 | if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) { |
saleiferis | 1:6e6f7e3cc1e1 | 186 | return; |
saleiferis | 1:6e6f7e3cc1e1 | 187 | } |
saleiferis | 1:6e6f7e3cc1e1 | 188 | |
saleiferis | 1:6e6f7e3cc1e1 | 189 | ble.gap().onDisconnection(disconnectionCallback); |
saleiferis | 4:4233f5538abf | 190 | ble.gap().onConnection(connectionCallback); |
saleiferis | 1:6e6f7e3cc1e1 | 191 | |
saleiferis | 1:6e6f7e3cc1e1 | 192 | /* Setup primary service. */ |
saleiferis | 19:f230229cb6f3 | 193 | hrServicePtr = new HeartRateService(ble, (uint8_t)hrmCounter, HeartRateService::LOCATION_FINGER); |
saleiferis | 19:f230229cb6f3 | 194 | //hrServicePtr = new ECGService(ble, hrmCounter); |
saleiferis | 5:30495ab95b86 | 195 | |
saleiferis | 4:4233f5538abf | 196 | pc.printf("Setup primary service ...\n"); |
saleiferis | 1:6e6f7e3cc1e1 | 197 | |
saleiferis | 1:6e6f7e3cc1e1 | 198 | /* Setup advertising. */ |
saleiferis | 1:6e6f7e3cc1e1 | 199 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); |
saleiferis | 1:6e6f7e3cc1e1 | 200 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); |
saleiferis | 1:6e6f7e3cc1e1 | 201 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR); |
saleiferis | 1:6e6f7e3cc1e1 | 202 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); |
saleiferis | 10:28b8729cf5dc | 203 | |
saleiferis | 1:6e6f7e3cc1e1 | 204 | ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
saleiferis | 1:6e6f7e3cc1e1 | 205 | ble.gap().setAdvertisingInterval(1000); /* 1000ms */ |
saleiferis | 10:28b8729cf5dc | 206 | |
saleiferis | 1:6e6f7e3cc1e1 | 207 | ble.gap().startAdvertising(); |
saleiferis | 10:28b8729cf5dc | 208 | |
saleiferis | 4:4233f5538abf | 209 | pc.printf("Exiting bleInitComplete() ... \n"); |
saleiferis | 4:4233f5538abf | 210 | return; |
saleiferis | 4:4233f5538abf | 211 | } |
saleiferis | 4:4233f5538abf | 212 | |
saleiferis | 1:6e6f7e3cc1e1 | 213 | |
saleiferis | 1:6e6f7e3cc1e1 | 214 | void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) { |
saleiferis | 1:6e6f7e3cc1e1 | 215 | BLE &ble = BLE::Instance(); |
saleiferis | 1:6e6f7e3cc1e1 | 216 | eventQueue.call(Callback<void()>(&ble, &BLE::processEvents)); |
saleiferis | 1:6e6f7e3cc1e1 | 217 | } |