![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
working wavelet transform
Dependencies: CMSIS_DSP_5 include mbed
Fork of Nucleo-Heart-Rate by
main.cpp@11:337c5ff16f27, 2018-06-22 (annotated)
- 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?
User | Revision | Line number | New 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 | } |