main.cpp

00001 #include "mbed.h"
00002 #include "mbed_events.h"
00003 #include "MAX30101.h"
00004 #include "string.h"
00005 #include "Hexi_OLED_SSD1351.h"
00006 #include "Hexi_KW40Z.h"
00007 
00008 #include "OLED_types.h"
00009 #include "OpenSans_Font.h"
00010 
00011 #include <math.h>
00012 #include <vector>
00013 using namespace std;
00014 
00015 #define FIFO_DATA_MAX 288
00016 //#define M_PI 3.14159265358979323846
00017 #define M_PI 3.14159
00018 
00019 void UpdateSensorData(void);
00020 void StartHaptic(void);
00021 void StopHaptic(void const *n);
00022 void txTask(void);
00023 
00024 DigitalOut blueLed(LED3,1); // i added this
00025 
00026 DigitalOut pwr1v8(PTA29);
00027 DigitalOut pwr3v3b(PTC13);
00028 DigitalOut pwr15v(PTB12);
00029 I2C i2c0(PTB1, PTB0);
00030 InterruptIn maximInterrupt(PTB18);
00031 Serial pc(USBTX, USBRX);
00032 SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15);
00033 KW40Z kw40z_device(PTE24, PTE25);
00034 DigitalOut haptic(PTB9);
00035 EventQueue evqueue(32 * EVENTS_EVENT_SIZE);
00036 Thread t;
00037 /* Define timer for haptic feedback */
00038 RtosTimer hapticTimer(StopHaptic, osTimerOnce);
00039 
00040 /*Create a Thread to handle sending BLE Sensor Data */
00041 Thread txThread;
00042 MAX30101 hr(i2c0);
00043 int ppg_single_sample;
00044 int mask_ppg = 0;
00045 uint32_t count = 0;
00046 uint32_t num;
00047 // uint8_t testsignal = 60;
00048 
00049 uint8_t battery = 0;
00050 
00051 char text[20];  /* Text Buffer */
00052 
00053 // I added this
00054 const int num_samples = 500; // for 5 sec
00055 int ppg[num_samples];
00056 int SDNN, valence_arousal;
00057 double HF_LF = 0.0;
00058 double HF_LF_n;
00059 int SDNN_n = 0;
00060 bool ready = false;
00061 bool ready_to_send = false;
00062 
00063 void StartHaptic(void)
00064 {
00065     hapticTimer.start(50);
00066     haptic = 1;
00067 }
00068 void ButtonRight(void)
00069 {
00070     StartHaptic();
00071     kw40z_device.ToggleAdvertisementMode();
00072 }
00073 
00074 void ButtonLeft(void)
00075 {
00076     StartHaptic();
00077     kw40z_device.ToggleAdvertisementMode();
00078 }
00079 
00080 void StopHaptic(void const *n)
00081 {
00082     haptic = 0;
00083     hapticTimer.stop();
00084 }
00085 
00086 void PassKey(void)
00087 {
00088     StartHaptic();
00089     strcpy((char *) text,"PAIR CODE");
00090     oled.TextBox((uint8_t *)text,0,25,95,18);
00091   
00092     /* Display Bond Pass Key in a 95px by 18px textbox at x=0,y=40 */
00093     sprintf(text,"%d", kw40z_device.GetPassKey());
00094     oled.TextBox((uint8_t *)text,0,40,95,18);
00095 }
00096 
00097 void txTask(void)
00098 {
00099 
00100     while (true) {
00101         /*
00102         while(ready_to_send == false) {
00103             Thread::wait(1000); // wait until has something to send
00104         }
00105 */
00106 
00107         if(ready_to_send) {
00108             UpdateSensorData();
00109             
00110             printf("Sending!");
00111             kw40z_device.SendBatteryLevel(battery);
00112             
00113             ready_to_send = false;
00114         }
00115         /*Notify Hexiwear App that it is running Sensor Tag mode*/
00116         kw40z_device.SendSetApplicationMode(GUI_CURRENT_APP_SENSOR_TAG);
00117         
00118         
00119         //send heartrate
00120 //        kw40z_device.SendHeartRate(testsignal);
00121         /*The following is sending dummy data over BLE. Replace with real data*/
00122 
00123         /*Send Battery Level for 20%
00124         kw40z_device.SendBatteryLevel(battery);
00125 
00126         Send Ambient Light Level at 50%
00127         kw40z_device.SendAmbientLight(light);*/
00128 
00129         /*Send Humidity at 90% */
00130         //kw40z_device.SendHumidity(humidity);
00131 
00132         /*Send Temperature at 25 degrees Celsius
00133         kw40z_device.SendTemperature(temperature);
00134 
00135         //Send Pressure at 100kPA 
00136         //kw40z_device.SendPressure(pressure); */
00137 
00138         /*Send Mag,Accel,Gyro Data.
00139         kw40z_device.SendGyro(x,y,z);
00140         kw40z_device.SendAccel(z,x,y);
00141         kw40z_device.SendMag(y,z,x);*/
00142 
00143         Thread::wait(1000);
00144     }
00145 }
00146 void UpdateSensorData(void)
00147 {
00148 //    testsignal+=1;
00149     battery = valence_arousal;
00150     /*battery -= 5;
00151     if(battery < 5) battery = 100;
00152 
00153     light += 20;
00154     if(light > 100) light = 0;
00155 
00156     humidity += 500;
00157     if(humidity > 8000) humidity = 2000;
00158 
00159     temperature -= 200;
00160     if(temperature < 200) temperature = 4200;
00161 
00162     pressure += 300;
00163     if(pressure > 10300) pressure = 7500;
00164 
00165     x += 1400;
00166     y -= 2300;
00167     z += 1700;*/
00168 }
00169 
00170 void interruptHandlerQueued()
00171 {
00172 //    int temp_ppg[num_samples];
00173     for(int iter = 0; iter < num_samples; iter+=0) {
00174         
00175         MAX30101::InterruptBitField_u interruptStatus;
00176         hr.getInterruptStatus(interruptStatus);
00177 //    printf("Interrupt Status: 0x%02x\r\n", interruptStatus.all);
00178 
00179         if (interruptStatus.bits.pwr_rdy == 0x1) {
00180 //        printf("Powered on\r\n");
00181 
00182             // Soft reset
00183             MAX30101::ModeConfiguration_u modeConf;
00184             modeConf.all = 0;
00185             modeConf.bits.reset = 1;
00186             hr.setModeConfiguration(modeConf);
00187             wait(0.01);
00188 
00189             // Configure FIFO
00190             MAX30101::FIFO_Configuration_u fifoConf;
00191             hr.getFIFOConfiguration(fifoConf);
00192 //        pc.printf("FIFO Configuration: 0x%02x\r\n", fifoConf.all);
00193 
00194             // Set LED power
00195             hr.setLEDPulseAmplitude(MAX30101::LED1_PA, 0x0C);
00196             hr.setLEDPulseAmplitude(MAX30101::ProxModeLED_PA, 0x19);
00197 //        pc.printf("LED set\r\n");
00198 
00199             MAX30101::SpO2Configuration_u spo2Conf;
00200             hr.getSpO2Configuration(spo2Conf);
00201             spo2Conf.bits.led_pw = MAX30101::PW_1;
00202             spo2Conf.bits.spo2_sr = MAX30101::SR_100_Hz;
00203             hr.setSpO2Configuration(spo2Conf);
00204             hr.getSpO2Configuration(spo2Conf);
00205 //        pc.printf("SpO2 Configuration: 0x%02x\r\n", spo2Conf.all);
00206 
00207             // Proximity settings
00208             hr.setProxIntThreshold(0x14);
00209 
00210             // Enable HR mode
00211             modeConf.all = 0;
00212             modeConf.bits.mode = MAX30101::HeartRateMode;
00213             hr.setModeConfiguration(modeConf);
00214 //        printf("Mode set\r\n");
00215         }
00216 
00217         if (interruptStatus.bits.prox_int == 0x1) {
00218 //        printf("Proximity Triggered, entered HR Mode.");
00219         }
00220 
00221         if (interruptStatus.bits.ppg_rdy == 0x1) {
00222 //        printf("PPG Ready.\r\n");
00223             mask_ppg = 1;
00224         }
00225 
00226         if (interruptStatus.bits.a_full == 0x1) {
00227 //        printf("FIFO Almost Full.\r\n");
00228             uint8_t data[FIFO_DATA_MAX];
00229             uint16_t readBytes = 0;
00230 
00231             hr.readFIFO(MAX30101::OneLedChannel, data, readBytes);
00232 //            printf("data length: %u \r\n",readBytes);
00233             //printf("data length: %u \r\n",data);
00234             for (uint16_t i = 0; i < readBytes; i += 3) {
00235                 uint8_t sample[4] = {0};
00236                 sample[0] = data[i + 2];
00237                 sample[1] = data[i + 1];
00238                 sample[2] = data[i];
00239 
00240                 num = *(uint32_t *) sample;
00241                 
00242                 if (num < 0) {
00243                     ppg_single_sample = 0;
00244 //                    printf("keep closer to your hand \r\n");
00245                 } else {
00246 
00247                     //ppg_single_sample = 65;
00248                     ppg_single_sample = num;
00249                     if(iter < num_samples && !ready) {
00250                         ppg[iter] = num;                        
00251 //                        printf("%d   \n", ppg[iter]);
00252                         if(iter == num_samples-1)
00253                             ready = true;
00254                     }
00255 
00256 //                    printf("%d   ", ppg_single_sample); // I commented this out
00257                     iter++;
00258                 }
00259 //                printf("%d; %d\r\n", iter, ppg[iter-1]);
00260 
00261 
00262             }
00263         }
00264 
00265         interruptStatus.all = 0xFF;
00266 
00267         if (mask_ppg == 1) {
00268             interruptStatus.bits.ppg_rdy = 0;
00269         }
00270         hr.enableInterrupts(interruptStatus);
00271     }
00272     /*
00273     for(int i = 0; i < num_samples; i++) {
00274         ppg[i] = temp_ppg[i];
00275     }
00276     */
00277     //ppg = temp_ppg;
00278 
00279 }
00280 
00281 void interruptHandler()
00282 {
00283     evqueue.call(interruptHandlerQueued);
00284 //    interruptHandlerQueued();
00285 //    printf("\nLezzgo\n");
00286     if(ready) {
00287         printf("\nStarting...  ");
00288 
00289         int i = 0;
00290         int j = 0;
00291         
00292         double TIME[num_samples];
00293         for(int i = 0; i < num_samples; i++) {
00294             TIME[i] = (double)i*(5.0/(double)num_samples); // change to 30.0 later
00295         }
00296         
00297         // moving average
00298         double movave[num_samples];
00299         int avecap = 25;
00300         for(i = 0; i < (avecap-1)/2; i++) {
00301             movave[i] = (double)ppg[0];
00302             for(j = 1; j < 1+(avecap-1)/2; j++) {
00303                 movave[i] = movave[i] + (double)ppg[j];
00304             }
00305             movave[i] = (double)(movave[i]/(i+(avecap-1)/2-1));
00306         }
00307         for(i = (num_samples-(avecap-1)/2); i < num_samples; i++) {
00308             movave[i] = (double)ppg[i-(avecap-1)/2-1];
00309             for(j = (i-(avecap-1)/2); j < num_samples; j++) {
00310                 movave[i] = movave[i] + (double)ppg[j];
00311             }
00312             movave[i] = movave[i]/(double)(num_samples-i+(avecap-1)/2);
00313         }
00314         for(i = (avecap-1)/2; i < (num_samples-(avecap-1)/2); i++) {
00315             movave[i] = (double)ppg[i-(avecap-1)/2-1];
00316             for(j = i-(avecap-1)/2; j < i+(avecap-1)/2; j++) {
00317                 movave[i] = movave[i] + (double)ppg[j];
00318             }
00319             movave[i] = movave[i]/(double)avecap;
00320         }
00321 
00322         // normalize ppg
00323         for(i = 0; i < num_samples; i++) {
00324             ppg[i] = ppg[i] - (int)movave[i];
00325             if(ppg[i] > 1000 || ppg[i] < -1000)
00326                 ppg[i] = 0;
00327         }
00328 
00329         // smoothing curve
00330         for(i = 1; i < num_samples; i++) {
00331             ppg[i] = ppg[i] + ppg[i-1];
00332             //printf("%d   ", ppg[i]);
00333         }
00334 
00335         // AMPD Algorithm
00336         const int kcap = 25;
00337         int m[kcap][num_samples];
00338         for(int k = 1; k <= kcap; k++) {
00339             for(i = 1; i < num_samples; i++) {
00340                 if(i-1 < 0 || i-k-1 < 0 || i+k-1 >= num_samples)
00341                     m[k-1][i] = 0;
00342                 else if(ppg[i-1] > ppg[i-k-1] && ppg[i-1] > ppg[i+k-1])
00343                     m[k-1][i] = 1;
00344                 else
00345                     m[k-1][i] = 0;
00346             }
00347         }
00348         
00349         int max_mult[num_samples];
00350         for(i = 0; i < num_samples; i++) { // max_mult = m(1,:)';
00351             max_mult[i] = m[0][i];
00352         }
00353 
00354         for(int k = 1; k < kcap; k++) {
00355             for(i = 0; i < num_samples; i++) {
00356                 max_mult[i] = max_mult[i]*m[k][i];
00357             }
00358         }
00359         
00360         int num_max = 0;
00361         // extract times that are max
00362         for(i = 0; i < num_samples; i++) {
00363             num_max = num_max + max_mult[i];
00364         }
00365 
00366 //    printf("num_max = %d    ", num_max);
00367 
00368         vector<double> time_of_max;
00369         vector<int> index_of_max;
00370         vector<int> max_points;
00371         for(i = 0; i < num_samples; i++) {
00372             if(max_mult[i] == 1) {
00373                 time_of_max.push_back(TIME[i-1]);
00374                 index_of_max.push_back(i-1);
00375                 max_points.push_back(ppg[i-1]);
00376             }
00377         }
00378 
00379         // calculating HRV
00380         vector<double> r;
00381         vector<int> index_r;
00382         double mean_inter_time = 0;
00383         for(i = 0; i < num_max-1; i++) {
00384             r.push_back(time_of_max.at(i+1)-time_of_max.at(i));
00385             index_r.push_back(index_of_max.at(i+1) - index_of_max.at(i));
00386             mean_inter_time = mean_inter_time + r.at(i);
00387         }
00388 
00389         //printf("r: %.2f %.2f %.2f   ", r.at(0), r.at(1), r.at(2));
00390         mean_inter_time = mean_inter_time/(double)(num_max-1);
00391 
00392         // getting rid of outlier points in r
00393         for(i = 0; i < num_max-1; i++) {
00394             if(r.at(i) > mean_inter_time + 0.11)
00395                 r.at(i) = mean_inter_time + 0.11;
00396             else if(r.at(i) < mean_inter_time - 0.11)
00397                 r.at(i) = mean_inter_time - 0.11;
00398         }
00399 
00400         // SDNN -- std of normal to normal R-R intervals
00401         mean_inter_time = 0;
00402         for(i = 0; i < num_max-1; i++) {
00403             mean_inter_time = mean_inter_time + r.at(i);
00404         }
00405         mean_inter_time = double(mean_inter_time/(num_max-1));
00406         double SDNN_doub = 0.0;
00407         for(i = 0; i < num_max-1; i++) {
00408             SDNN_doub = SDNN_doub + (r.at(i)-mean_inter_time)*(r.at(i)-mean_inter_time);
00409         }
00410         SDNN = (int)(sqrt(SDNN_doub/(num_max-1))*1000);
00411         printf("SDNN = %d    ", SDNN);
00412 
00413         // TIME TO CALCULATE HF/LF
00414         // FFT: use movave as fftppg 
00415         for(i = 0; i < num_samples; i++) {
00416             double real = 0;
00417             double im = 0;
00418             double dum_ppg;
00419             for(j = 0; j < num_samples; j++) {
00420                 dum_ppg = (double) ppg[j];
00421                 real = real + dum_ppg * cos(2.0*M_PI*(double)(j*i)/(double)num_samples);
00422                 im = im + dum_ppg * sin(2.0*M_PI*(double)(j*i)/(double)num_samples);
00423             }
00424             movave[i] = sqrt(real*real + im*im)/(double)num_samples;
00425         }
00426         
00427         
00428         // make frequency array: use TIME
00429         double sampling_freq = (double)(1/TIME[1]);
00430         for(i = 0; i < num_samples; i++) {
00431             TIME[i] = sampling_freq*(double)(i)/(double)(num_samples);
00432         }
00433 
00434         double LF = 0.0;
00435         i = 0;
00436         while(TIME[i] < 0.15) {
00437             LF = LF + movave[i];
00438             i++;
00439         }
00440 
00441         double HF = 0.0;
00442         while(TIME[i] < 0.4) {
00443             HF = HF + movave[i];
00444             i++;
00445         }
00446         HF_LF = HF/LF;
00447         printf("HF/LF = %.2f    \n", HF_LF);
00448         
00449         if(SDNN_n < 1) {
00450             SDNN_n = SDNN;
00451             HF_LF_n = HF_LF;
00452             valence_arousal = 0;
00453         }
00454         else {
00455             // mapping SDNN, HF/LF to valence, arousal
00456             int comp_sdnn = SDNN-SDNN_n;
00457             if(comp_sdnn > 20)
00458                 valence_arousal = 5;
00459             else if(comp_sdnn > 5)
00460                 valence_arousal = 4;
00461             else if(comp_sdnn > -5)
00462                 valence_arousal = 3;
00463             else if(comp_sdnn > -20)
00464                 valence_arousal = 2;
00465             else
00466                 valence_arousal = 1;
00467             
00468             double comp_hf_lf = HF_LF/HF_LF_n;
00469             if(comp_hf_lf > 5.0)
00470                 valence_arousal += 50;
00471             else if(comp_hf_lf > 1.5)
00472                 valence_arousal += 40;
00473             else if(comp_hf_lf > 0.9)
00474                 valence_arousal += 30;
00475             else if(comp_hf_lf > 0.7)
00476                 valence_arousal += 20;
00477             else
00478                 valence_arousal += 10;
00479             ready_to_send = true;
00480             printf("valence_arousal = %d   ", valence_arousal);
00481             
00482         }
00483         
00484         ready = false; // last line
00485 
00486     }
00487 
00488 }
00489 
00490 // main() runs in its own thread in the OS
00491 int main()
00492 {
00493 //    printf("Hello world.\r\n");
00494     
00495     t.start(callback(&evqueue, &EventQueue::dispatch_forever));
00496     kw40z_device.attach_buttonLeft(&ButtonLeft);
00497     kw40z_device.attach_buttonRight(&ButtonRight);
00498     kw40z_device.attach_passkey(&PassKey);
00499 
00500     pwr1v8 = 1;
00501     pwr3v3b = 1;
00502     pwr15v = 0;
00503 
00504     maximInterrupt.fall(interruptHandler);
00505     maximInterrupt.enable_irq();
00506 
00507     MAX30101::InterruptBitField_u interruptStatus;
00508     interruptStatus.all = 0xFF;
00509     hr.enableInterrupts(interruptStatus);
00510 
00511     
00512     oled_text_properties_t textProperties = {0};
00513     oled.GetTextProperties(&textProperties);
00514     /* Turn on the backlight of the OLED Display */
00515     oled.DimScreenON();
00516 
00517     /* Fills the screen with solid black */
00518     oled.FillScreen(COLOR_BLACK);
00519     strcpy((char *) text, "Raw PPG:");
00520     oled.Label((uint8_t *)text,7,0);
00521 
00522     strcpy((char *) text, "         LOFI");
00523     oled.Label((uint8_t *)text,7,40);
00524     //dynamic text setup
00525     textProperties.fontColor = COLOR_WHITE;
00526     textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
00527     oled.SetTextProperties(&textProperties);
00528 
00529     txThread.start(txTask);
00530     
00531     // added below
00532     
00533     /* Change font color to Blue */ 
00534     textProperties.fontColor   = COLOR_BLUE;
00535     oled.SetTextProperties(&textProperties);
00536     
00537     /* Display Bluetooth Label at x=17,y=65 */ 
00538     strcpy((char *) text,"BLUETOOTH");
00539     oled.Label((uint8_t *)text,17,65);
00540     
00541     /* Change font color to white */ 
00542     textProperties.fontColor   = COLOR_WHITE;
00543     textProperties.alignParam = OLED_TEXT_ALIGN_CENTER;
00544     oled.SetTextProperties(&textProperties);
00545     
00546     /* Display Label at x=22,y=80 */ 
00547     strcpy((char *) text,"Tap to turn OFF");
00548     oled.Label((uint8_t *)text,7,80);
00549     
00550     // added above
00551     
00552     while (true) {
00553 
00554         // Format the time reading 
00555         sprintf(text,"%d",ppg_single_sample);
00556         oled.TextBox((uint8_t *)text,55,15,35,15); //Increase textbox for more digits
00557         
00558 /*
00559         // Display time reading in 35px by 15px textbox at(x=55, y=40) 
00560         if (ppg_single_sample > 45) {
00561             sprintf(text,"%d; %.2f",SDNN, HF_LF);
00562 
00563             // Display time reading in 35px by 15px textbox at(x=55, y=40) 
00564             oled.TextBox((uint8_t *)text,55,55,35,15); //Increase textbox for more digits
00565         } else {
00566             sprintf(text,"wait HR");
00567 
00568             // Display time reading in 35px by 15px textbox at(x=55, y=40) 
00569             oled.TextBox((uint8_t *)text,55,55,35,15);
00570         }*/
00571         
00572         blueLed = !kw40z_device.GetAdvertisementMode();  // added this
00573         
00574         Thread::wait(1000);
00575     }
00576     return 0;
00577 }
00578