File content as of revision 11:337c5ff16f27:

#include "arm_common_tables.h"
#include "arm_const_structs.h"
#include "arm_math.h"
#include "mbed.h"
#include "vals.h"
#include "../header/wavelib.h"



DigitalOut myled(LED1);
DigitalOut myled2(LED2);

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()
{
    usb_serial.baud(baud_rate);
    // Set serial USB connection baud rate (variable is declared in config part).
    wave_object wave_init_obj;
    wt_object wavelet;

    int N_samples = 2000;
    int wt_scale = 3;
    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

    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) {
            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<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;
        }
    
//    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) {
//            usb_serial.printf("%d %e \n",i, wavelet->output[i]);
//        }
//        myled = !myled;
//    }



    //Algoritme

}