working wavelet transform

Dependencies:   CMSIS_DSP_5 include mbed

Fork of Nucleo-Heart-Rate by BAP TUDelft

Committer:
MockyBirdTwo
Date:
Fri Jun 22 13:46:16 2018 +0000
Revision:
11:337c5ff16f27
Parent:
10:f62efa525bb6
algorithm is correct now;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xorjoep 0:f39864a2bd26 1 #include "arm_common_tables.h"
xorjoep 0:f39864a2bd26 2 #include "arm_const_structs.h"
xorjoep 0:f39864a2bd26 3 #include "arm_math.h"
xorjoep 0:f39864a2bd26 4 #include "mbed.h"
xorjoep 2:3d6a6b9afee0 5 #include "vals.h"
xorjoep 0:f39864a2bd26 6 #include "../header/wavelib.h"
xorjoep 0:f39864a2bd26 7
MockyBirdTwo 10:f62efa525bb6 8
MockyBirdTwo 10:f62efa525bb6 9
xorjoep 0:f39864a2bd26 10 DigitalOut myled(LED1);
xorjoep 4:8bd1cecf2a2e 11 DigitalOut myled2(LED2);
xorjoep 0:f39864a2bd26 12
xorjoep 4:8bd1cecf2a2e 13 Serial usb_serial(SERIAL_TX, SERIAL_RX); // tx, rx
xorjoep 4:8bd1cecf2a2e 14 const int baud_rate = 115200; // Baud rate.
MockyBirdTwo 11:337c5ff16f27 15 double* getPeaks(double* values, int window, double Tr_low)
MockyBirdTwo 11:337c5ff16f27 16 {
MockyBirdTwo 11:337c5ff16f27 17 static double result[4] = {0, 0, 0, 0}; // {Value min, index min, value max, index max}
MockyBirdTwo 11:337c5ff16f27 18 int wnd_max = 320;//Max window is 2 seconds = 2*fs = 320
MockyBirdTwo 11:337c5ff16f27 19 int cont =0;// Variable to continue to max peak find
MockyBirdTwo 11:337c5ff16f27 20 int i,j;
MockyBirdTwo 11:337c5ff16f27 21 for(i = 0; i < wnd_max; i++) {
MockyBirdTwo 11:337c5ff16f27 22 if(values[i] < Tr_low) { //Find MIN peak below a threshold
MockyBirdTwo 11:337c5ff16f27 23 result[0] = values[i]; // Place where the threshold is met
MockyBirdTwo 11:337c5ff16f27 24 cont=1; //Continue to find MAX peak
MockyBirdTwo 11:337c5ff16f27 25 for(j=i; j<i+window; j++) {//Find MIN peak
MockyBirdTwo 11:337c5ff16f27 26 if(values[j] < result[0]) {
MockyBirdTwo 11:337c5ff16f27 27 result[0] = values[j]; //value MIN peak
MockyBirdTwo 11:337c5ff16f27 28 result[1] = (double) j; //index MIN peak
MockyBirdTwo 11:337c5ff16f27 29 }
MockyBirdTwo 11:337c5ff16f27 30 }
MockyBirdTwo 11:337c5ff16f27 31 break;
MockyBirdTwo 11:337c5ff16f27 32 }
MockyBirdTwo 11:337c5ff16f27 33 }
MockyBirdTwo 11:337c5ff16f27 34 if (cont==1) {
MockyBirdTwo 11:337c5ff16f27 35 for(i = result[1]; i < result[1]+window; i++) {
MockyBirdTwo 11:337c5ff16f27 36 result[2]=0;
MockyBirdTwo 11:337c5ff16f27 37 if (values[i]>result[2]) {
MockyBirdTwo 11:337c5ff16f27 38 result[2]=values[i];
MockyBirdTwo 11:337c5ff16f27 39 result[3]=(double) i;
MockyBirdTwo 11:337c5ff16f27 40 }
MockyBirdTwo 11:337c5ff16f27 41 }
MockyBirdTwo 11:337c5ff16f27 42 }
MockyBirdTwo 11:337c5ff16f27 43
MockyBirdTwo 11:337c5ff16f27 44 return result;//Will return result = {0,0,0,0} if nothing is found
MockyBirdTwo 11:337c5ff16f27 45 }
xorjoep 0:f39864a2bd26 46
MockyBirdTwo 10:f62efa525bb6 47 int main()
MockyBirdTwo 10:f62efa525bb6 48 {
MockyBirdTwo 10:f62efa525bb6 49 usb_serial.baud(baud_rate);
MockyBirdTwo 10:f62efa525bb6 50 // Set serial USB connection baud rate (variable is declared in config part).
MockyBirdTwo 10:f62efa525bb6 51 wave_object wave_init_obj;
xorjoep 4:8bd1cecf2a2e 52 wt_object wavelet;
xorjoep 4:8bd1cecf2a2e 53
MockyBirdTwo 10:f62efa525bb6 54 int N_samples = 2000;
xorjoep 4:8bd1cecf2a2e 55 int wt_scale = 3;
MockyBirdTwo 6:ce7f5faea04a 56 int i,j;
MockyBirdTwo 11:337c5ff16f27 57 static int fs =160;//Sampling freq.
MockyBirdTwo 11:337c5ff16f27 58 int wnd_offset = 0.25*fs;
MockyBirdTwo 11:337c5ff16f27 59 static int wnd=0.05*fs;//Window to find peaks in
MockyBirdTwo 11:337c5ff16f27 60 int Strtup =1;// Start with the starting sequence
MockyBirdTwo 11:337c5ff16f27 61 double Tr_low = -0.02; //Threshold MIN peak
MockyBirdTwo 11:337c5ff16f27 62 double Tr_temp = 0; //Temporary threshold MIN peak
MockyBirdTwo 11:337c5ff16f27 63 double Tr_scale = 0.45; //Starting threshold scale
MockyBirdTwo 11:337c5ff16f27 64 double *Peaks=(double*)calloc(4,sizeof(double)); //To save peak data
MockyBirdTwo 11:337c5ff16f27 65 double *PTD=(double*)calloc(3,sizeof(double)); //To save peak time difference
MockyBirdTwo 11:337c5ff16f27 66 int *Index_min=(int*)calloc(4,sizeof(int)); //To save MIN peak index
MockyBirdTwo 11:337c5ff16f27 67 int *Index_max=(int*)calloc(4,sizeof(int)); //To save MAX peak index
MockyBirdTwo 11:337c5ff16f27 68 double PTD_temp;//Temporary value of PTD
MockyBirdTwo 11:337c5ff16f27 69 double MTD2;//PTD average of last two PTD
MockyBirdTwo 11:337c5ff16f27 70 double MTD3;//PTD average of last 3 PTD
xorjoep 4:8bd1cecf2a2e 71 double *inp = signal_in; //,*out,*diff;
MockyBirdTwo 10:f62efa525bb6 72 double *out =(double*)malloc(sizeof(double)* N_samples);
MockyBirdTwo 10:f62efa525bb6 73 double *wt_res=(double*)malloc(sizeof(double)* N_samples);
MockyBirdTwo 11:337c5ff16f27 74 double *t_wavelet=(double*)malloc(sizeof(double)* N_samples); //To save wavelet coefficients temporarily
MockyBirdTwo 11:337c5ff16f27 75
MockyBirdTwo 11:337c5ff16f27 76
xorjoep 4:8bd1cecf2a2e 77 char *m_wavelet = "db4";
MockyBirdTwo 11:337c5ff16f27 78
xorjoep 4:8bd1cecf2a2e 79 wave_init_obj = wave_init(m_wavelet);// Initialize the wavelet
MockyBirdTwo 10:f62efa525bb6 80
xorjoep 4:8bd1cecf2a2e 81 wavelet = wt_init(wave_init_obj, "modwt", N_samples, wt_scale);// Initialize the wavelet transform object
MockyBirdTwo 10:f62efa525bb6 82
xorjoep 4:8bd1cecf2a2e 83 modwt(wavelet, inp);// Perform MODW
MockyBirdTwo 10:f62efa525bb6 84
MockyBirdTwo 10:f62efa525bb6 85 int wt_scale_start=2000;//Level 3
MockyBirdTwo 10:f62efa525bb6 86 int wt_scale_end=4000;
MockyBirdTwo 6:ce7f5faea04a 87 //Choose scales
MockyBirdTwo 10:f62efa525bb6 88 for (i=0,j=wt_scale_start; j<wt_scale_end; i++,j++) {
MockyBirdTwo 10:f62efa525bb6 89 t_wavelet[i]=wavelet->output[j];
MockyBirdTwo 6:ce7f5faea04a 90 }
MockyBirdTwo 11:337c5ff16f27 91
MockyBirdTwo 10:f62efa525bb6 92 wavelet->output=t_wavelet;
MockyBirdTwo 9:854f2d4eda4a 93 imodwt(wavelet,out);// Perform IMODWT
MockyBirdTwo 10:f62efa525bb6 94 for (i=0; i<N_samples; ++i) {
MockyBirdTwo 10:f62efa525bb6 95 wt_res[i]=wavelet->output[i]*fabs(wavelet->output[i]);
MockyBirdTwo 10:f62efa525bb6 96 }
MockyBirdTwo 11:337c5ff16f27 97 while (1) {
MockyBirdTwo 11:337c5ff16f27 98 if (Strtup == 1) { //Starting sequence
MockyBirdTwo 11:337c5ff16f27 99 //First heartbeat
MockyBirdTwo 11:337c5ff16f27 100 Peaks = getPeaks(wt_res, wnd, Tr_low);// Peaks={Value min, index min, value max, index max}
MockyBirdTwo 11:337c5ff16f27 101 Tr_temp = Tr_scale*Peaks[0]; //Set MIN peak threshold
MockyBirdTwo 11:337c5ff16f27 102 Index_min[0]= (int)Peaks[1]; //Save MIN peak index
MockyBirdTwo 11:337c5ff16f27 103 Index_max[0]=(int)Peaks[3]; //Save MAX peak index
MockyBirdTwo 10:f62efa525bb6 104
MockyBirdTwo 11:337c5ff16f27 105 for (i=0; i<3; i++) { //Next 3 heartbeats
MockyBirdTwo 11:337c5ff16f27 106 Peaks = getPeaks(wt_res+(wnd_offset+Index_min[i]), wnd, Tr_temp);// Peaks={Value min, index min, value max, index max}
MockyBirdTwo 11:337c5ff16f27 107 Tr_temp = Tr_scale*Peaks[0];//Set MIN peak threshold
MockyBirdTwo 11:337c5ff16f27 108 Index_min[i+1]=wnd_offset+Index_min[i]+(int)Peaks[1];//Save MIN peak index + starting offset
MockyBirdTwo 11:337c5ff16f27 109 Index_max[i+1]=wnd_offset+Index_min[i]+(int)Peaks[3];//Save MAX peak index + starting offset
MockyBirdTwo 11:337c5ff16f27 110 PTD[i] = Index_max[i+1]-Index_max[i];//Determine PTD
MockyBirdTwo 11:337c5ff16f27 111 wnd_offset = 0.5*PTD[i]; //Set new window offset
MockyBirdTwo 11:337c5ff16f27 112 }
MockyBirdTwo 11:337c5ff16f27 113 //Set MTD2 & MTD3
MockyBirdTwo 11:337c5ff16f27 114 MTD2 = (PTD[2]+PTD[1])/2;
MockyBirdTwo 11:337c5ff16f27 115 MTD3 = (PTD[2]+PTD[1]+PTD[0])/3;
MockyBirdTwo 11:337c5ff16f27 116 if(MTD2>(0.9*MTD3) && MTD2<(1.1*MTD3)) { //Signal stable, so heartbeat found
MockyBirdTwo 11:337c5ff16f27 117 Strtup = 0;
MockyBirdTwo 11:337c5ff16f27 118 Tr_low =Tr_temp;
MockyBirdTwo 11:337c5ff16f27 119 } else { //Unstable signal, restart startup
MockyBirdTwo 11:337c5ff16f27 120 Strtup = 1;
MockyBirdTwo 11:337c5ff16f27 121 Tr_low = -0.01;
MockyBirdTwo 11:337c5ff16f27 122 }
MockyBirdTwo 11:337c5ff16f27 123
MockyBirdTwo 11:337c5ff16f27 124 } else if(Strtup==0) { //Normal sequence
MockyBirdTwo 11:337c5ff16f27 125 //First heartbeat
MockyBirdTwo 11:337c5ff16f27 126 Peaks = getPeaks(wt_res, wnd, Tr_low);// Peaks={Value min, index min, value max, index max}
MockyBirdTwo 11:337c5ff16f27 127 Tr_temp = Tr_scale*Peaks[0]; //Set MIN peak threshold
MockyBirdTwo 11:337c5ff16f27 128 Index_min[0]=(int)Peaks[1]; //Save MIN peak index
MockyBirdTwo 11:337c5ff16f27 129 Index_max[0]=(int)Peaks[3]; //Save MAX peak index
MockyBirdTwo 11:337c5ff16f27 130 usb_serial.printf("Normal sequence started \n");
MockyBirdTwo 11:337c5ff16f27 131 for (i=0; i<2; i++) {
MockyBirdTwo 11:337c5ff16f27 132 Peaks = getPeaks(wt_res+(wnd_offset+Index_min[i]), wnd, Tr_temp);// Peaks={Value min, index min, value max, index max}
MockyBirdTwo 11:337c5ff16f27 133 Tr_temp = Tr_scale*Peaks[0]; //Set MIN peak threshold
MockyBirdTwo 11:337c5ff16f27 134 Index_min[i+1]=wnd_offset+Index_min[i]+(int)Peaks[1]; //Save MIN peak index
MockyBirdTwo 11:337c5ff16f27 135 Index_max[i+1]=wnd_offset+Index_min[i]+(int)Peaks[3]; //Save MAX peak index
MockyBirdTwo 11:337c5ff16f27 136
MockyBirdTwo 11:337c5ff16f27 137 PTD_temp=Index_max[i+1]-Index_max[i];
MockyBirdTwo 11:337c5ff16f27 138 if (PTD_temp>= (0.8*MTD2) && PTD_temp<= (1.2*MTD2)) {//Found heart rate
MockyBirdTwo 11:337c5ff16f27 139 Strtup = 0;//Continue normal sequence
MockyBirdTwo 11:337c5ff16f27 140 Tr_low =Tr_temp; //Set threshold
MockyBirdTwo 11:337c5ff16f27 141 //Pop off first PTD and add PTD_temp to the back of PTD
MockyBirdTwo 11:337c5ff16f27 142 PTD[1]=PTD[2];
MockyBirdTwo 11:337c5ff16f27 143 PTD[0]=PTD[1];
MockyBirdTwo 11:337c5ff16f27 144 PTD[2]=PTD_temp;
MockyBirdTwo 11:337c5ff16f27 145
MockyBirdTwo 11:337c5ff16f27 146 //Set MTD2 & MTD3
MockyBirdTwo 11:337c5ff16f27 147 MTD2 = (PTD[2]+PTD[1])/2;
MockyBirdTwo 11:337c5ff16f27 148 MTD3 = (PTD[2]+PTD[1]+PTD[0])/2;
MockyBirdTwo 11:337c5ff16f27 149
MockyBirdTwo 11:337c5ff16f27 150 break;
MockyBirdTwo 11:337c5ff16f27 151 }
MockyBirdTwo 11:337c5ff16f27 152 Strtup = 1;
MockyBirdTwo 11:337c5ff16f27 153 }
MockyBirdTwo 11:337c5ff16f27 154 } else { //Error
MockyBirdTwo 11:337c5ff16f27 155 usb_serial.printf("Error in algorithm");
MockyBirdTwo 11:337c5ff16f27 156 }
MockyBirdTwo 11:337c5ff16f27 157 break;
xorjoep 0:f39864a2bd26 158 }
MockyBirdTwo 11:337c5ff16f27 159
MockyBirdTwo 11:337c5ff16f27 160 // while(1) {
MockyBirdTwo 11:337c5ff16f27 161 // for (i = 0; i < N_samples; ++i) {
MockyBirdTwo 11:337c5ff16f27 162 // usb_serial.printf("%d %e \n",i, wt_res[i]);
MockyBirdTwo 11:337c5ff16f27 163 // }
MockyBirdTwo 11:337c5ff16f27 164 // myled = !myled;
MockyBirdTwo 11:337c5ff16f27 165 // }
MockyBirdTwo 10:f62efa525bb6 166 // while(1) {
MockyBirdTwo 10:f62efa525bb6 167 //
MockyBirdTwo 10:f62efa525bb6 168 // for (i = 0; i < N_samples; ++i) {
MockyBirdTwo 10:f62efa525bb6 169 // usb_serial.printf("%d %e \n",i, wavelet->output[i]);
MockyBirdTwo 10:f62efa525bb6 170 // }
MockyBirdTwo 10:f62efa525bb6 171 // myled = !myled;
MockyBirdTwo 10:f62efa525bb6 172 // }
MockyBirdTwo 10:f62efa525bb6 173
MockyBirdTwo 10:f62efa525bb6 174
MockyBirdTwo 10:f62efa525bb6 175
MockyBirdTwo 10:f62efa525bb6 176 //Algoritme
MockyBirdTwo 10:f62efa525bb6 177
xorjoep 0:f39864a2bd26 178 }