Diff: main.cpp

Revision:

11:337c5ff16f27

Parent:

10:f62efa525bb6

--- a/main.cpp	Thu Jun 21 11:55:09 2018 +0000
+++ b/main.cpp	Fri Jun 22 13:46:16 2018 +0000
@@ -12,6 +12,37 @@
 
 Serial usb_serial(SERIAL_TX, SERIAL_RX);    // tx, rx
 const int baud_rate = 115200;     // Baud rate.
+double* getPeaks(double* values, int window, double Tr_low)
+{
+    static double result[4] = {0, 0, 0, 0}; // {Value min, index min, value max, index max}
+    int wnd_max = 320;//Max window is 2 seconds = 2*fs = 320
+    int cont =0;// Variable to continue to max peak find
+    int i,j;
+    for(i = 0; i < wnd_max; i++) {
+        if(values[i] < Tr_low) { //Find MIN peak below a threshold
+            result[0] = values[i]; // Place where the threshold is met
+            cont=1; //Continue to find MAX peak
+            for(j=i; j<i+window; j++) {//Find MIN peak
+                if(values[j] < result[0]) {
+                    result[0] = values[j];  //value MIN peak
+                    result[1] = (double) j; //index MIN peak
+                }
+            }
+            break;
+        }
+    }
+    if (cont==1) {
+        for(i = result[1]; i < result[1]+window; i++) {
+            result[2]=0;
+            if (values[i]>result[2]) {
+                result[2]=values[i];
+                result[3]=(double) i;
+            }
+        }
+    }
+
+    return result;//Will return result = {0,0,0,0} if nothing is found
+}
 
 int main()
 {
@@ -22,46 +53,116 @@
 
     int N_samples = 2000;
     int wt_scale = 3;
-//    int fs = 160;
     int i,j;
-   
-    
+    static int fs =160;//Sampling freq.
+    int wnd_offset = 0.25*fs;
+    static int wnd=0.05*fs;//Window to find peaks in
+    int Strtup =1;// Start with the starting sequence
+    double Tr_low = -0.02; //Threshold MIN peak
+    double Tr_temp = 0; //Temporary threshold MIN peak
+    double Tr_scale = 0.45; //Starting threshold scale
+    double *Peaks=(double*)calloc(4,sizeof(double)); //To save peak data
+    double *PTD=(double*)calloc(3,sizeof(double)); //To save peak time difference
+    int *Index_min=(int*)calloc(4,sizeof(int)); //To save MIN peak index
+    int *Index_max=(int*)calloc(4,sizeof(int)); //To save MAX peak index
+    double PTD_temp;//Temporary value of PTD
+    double MTD2;//PTD average of last two PTD
+    double MTD3;//PTD average of last 3 PTD
     double *inp = signal_in; //,*out,*diff;
     double *out =(double*)malloc(sizeof(double)* N_samples);
     double *wt_res=(double*)malloc(sizeof(double)* N_samples);
-     
+    double *t_wavelet=(double*)malloc(sizeof(double)* N_samples); //To save wavelet coefficients temporarily
+
+        
     char *m_wavelet = "db4";
+    
     wave_init_obj = wave_init(m_wavelet);// Initialize the wavelet
 
     wavelet = wt_init(wave_init_obj, "modwt", N_samples, wt_scale);// Initialize the wavelet transform object
 
     modwt(wavelet, inp);// Perform MODW
-    wt_summary(wavelet);
-    double t_wavelet[N_samples*wt_scale+2000];
-    memset(t_wavelet,0,N_samples*wt_scale+2000);
 
-//    int wt_scale_start=((wavelet->outlength)/wt_scale)*0;//1=last level
-//    int wt_scale_end=((wavelet->outlength)/wt_scale)*2;
     int wt_scale_start=2000;//Level 3
     int wt_scale_end=4000;
     //Choose scales
     for (i=0,j=wt_scale_start; j<wt_scale_end; i++,j++) {
         t_wavelet[i]=wavelet->output[j];
     }
+
     wavelet->output=t_wavelet;
     imodwt(wavelet,out);// Perform IMODWT
-
     for (i=0; i<N_samples; ++i) {
         wt_res[i]=wavelet->output[i]*fabs(wavelet->output[i]);
     }
-    
-    while(1) {
+while (1) {
+            if (Strtup == 1) { //Starting sequence
+                //First heartbeat
+                Peaks = getPeaks(wt_res, wnd, Tr_low);// Peaks={Value min, index min, value max, index max}               
+                Tr_temp = Tr_scale*Peaks[0]; //Set MIN peak threshold
+                Index_min[0]= (int)Peaks[1]; //Save MIN peak index
+                Index_max[0]=(int)Peaks[3]; //Save MAX peak index
 
-        for (i = 0; i < N_samples; ++i) {
-            usb_serial.printf("%d %e \n",i, wt_res[i]);
+                for (i=0; i<3; i++) { //Next 3 heartbeats
+                    Peaks = getPeaks(wt_res+(wnd_offset+Index_min[i]), wnd, Tr_temp);// Peaks={Value min, index min, value max, index max}
+                    Tr_temp = Tr_scale*Peaks[0];//Set MIN peak threshold
+                    Index_min[i+1]=wnd_offset+Index_min[i]+(int)Peaks[1];//Save MIN peak index + starting offset
+                    Index_max[i+1]=wnd_offset+Index_min[i]+(int)Peaks[3];//Save MAX peak index + starting offset
+                    PTD[i] = Index_max[i+1]-Index_max[i];//Determine PTD
+                    wnd_offset = 0.5*PTD[i]; //Set new window offset
+                }
+                //Set MTD2 & MTD3
+                MTD2 = (PTD[2]+PTD[1])/2;
+                MTD3 = (PTD[2]+PTD[1]+PTD[0])/3;
+                if(MTD2>(0.9*MTD3) && MTD2<(1.1*MTD3)) { //Signal stable, so heartbeat found
+                    Strtup = 0;
+                    Tr_low =Tr_temp;
+                } else { //Unstable signal, restart startup
+                    Strtup = 1;
+                    Tr_low = -0.01;
+                }
+                
+            } else if(Strtup==0) { //Normal sequence
+                //First heartbeat
+                Peaks = getPeaks(wt_res, wnd, Tr_low);// Peaks={Value min, index min, value max, index max}
+                Tr_temp = Tr_scale*Peaks[0]; //Set MIN peak threshold
+                Index_min[0]=(int)Peaks[1]; //Save MIN peak index
+                Index_max[0]=(int)Peaks[3]; //Save MAX peak index     
+usb_serial.printf("Normal sequence started \n");        
+                for (i=0; i<2; i++) {
+                    Peaks = getPeaks(wt_res+(wnd_offset+Index_min[i]), wnd, Tr_temp);// Peaks={Value min, index min, value max, index max}
+                    Tr_temp = Tr_scale*Peaks[0]; //Set MIN peak threshold
+                    Index_min[i+1]=wnd_offset+Index_min[i]+(int)Peaks[1]; //Save MIN peak index
+                    Index_max[i+1]=wnd_offset+Index_min[i]+(int)Peaks[3]; //Save MAX peak index 
+                    
+                    PTD_temp=Index_max[i+1]-Index_max[i];
+                    if (PTD_temp>= (0.8*MTD2) && PTD_temp<= (1.2*MTD2)) {//Found heart rate
+                        Strtup = 0;//Continue normal sequence 
+                        Tr_low =Tr_temp; //Set threshold
+                        //Pop off first PTD and add PTD_temp to the back of PTD
+                        PTD[1]=PTD[2];
+                        PTD[0]=PTD[1];
+                        PTD[2]=PTD_temp;
+                        
+                        //Set MTD2 & MTD3
+                        MTD2 = (PTD[2]+PTD[1])/2;
+                        MTD3 = (PTD[2]+PTD[1]+PTD[0])/2;
+        
+                        break; 
+                    }
+                    Strtup = 1;
+                }
+            } else { //Error
+                usb_serial.printf("Error in algorithm");
+            }
+            break;
         }
-        myled = !myled;
-    }
+    
+//    while(1) {
+//        for (i = 0; i < N_samples; ++i) {
+//            usb_serial.printf("%d %e \n",i, wt_res[i]);
+//        }
+//        myled = !myled;
+//    }
  //   while(1) {
 //
 //        for (i = 0; i < N_samples; ++i) {