File content as of revision 6:ff1c3560db84:

#include "mbed.h"
#include "mbed_events.h"
#include "MAX30101.h"
#include "string.h"
#include "Hexi_OLED_SSD1351.h"
#include "Hexi_KW40Z.h"
#include <math.h>
#include <vector>
using namespace std;
#define FIFO_DATA_MAX 288
void UpdateSensorData(void);
void StartHaptic(void);
void StopHaptic(void const *n);
void txTask(void);

DigitalOut pwr1v8(PTA29);
DigitalOut pwr3v3b(PTC13);
DigitalOut pwr15v(PTB12);
I2C i2c0(PTB1, PTB0);
InterruptIn maximInterrupt(PTB18);
Serial pc(USBTX, USBRX);
SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15);
KW40Z kw40z_device(PTE24, PTE25);
DigitalOut haptic(PTB9);
EventQueue evqueue(32 * EVENTS_EVENT_SIZE);
Thread t;
/* Define timer for haptic feedback */
RtosTimer hapticTimer(StopHaptic, osTimerOnce);

/*Create a Thread to handle sending BLE Sensor Data */
Thread txThread;
MAX30101 hr(i2c0);
int ppg_single_sample;
int mask_ppg = 0;
uint32_t count = 0;
uint32_t num;
uint8_t testsignal = 60;

// I added this
const int num_samples = 1200; // for 30 sec
double TIME[num_samples];
int ppg[num_samples];
int SDNN_n, SDNN;
bool first_sample_set = true;
double arousal, valence, HF_LF, HF_LF_n;

void StartHaptic(void)
{
    hapticTimer.start(50);
    haptic = 1;
}
void ButtonRight(void)
{
    StartHaptic();
    kw40z_device.ToggleAdvertisementMode();
}

void ButtonLeft(void)
{
    StartHaptic();
    kw40z_device.ToggleAdvertisementMode();
}

void StopHaptic(void const *n)
{
    haptic = 0;
    hapticTimer.stop();
}
void txTask(void)
{

    while (true) {
        UpdateSensorData();

        /*Notify Hexiwear App that it is running Sensor Tag mode*/
        kw40z_device.SendSetApplicationMode(GUI_CURRENT_APP_SENSOR_TAG);
        //send heartrate
        kw40z_device.SendHeartRate(testsignal);
        /*The following is sending dummy data over BLE. Replace with real data*/

        /*Send Battery Level for 20%
        kw40z_device.SendBatteryLevel(battery);

        Send Ambient Light Level at 50%
        kw40z_device.SendAmbientLight(light);*/

        /*Send Humidity at 90% */
        //kw40z_device.SendHumidity(humidity);

        /*Send Temperature at 25 degrees Celsius
        kw40z_device.SendTemperature(temperature);

        /*Send Pressure at 100kPA */
        //kw40z_device.SendPressure(pressure);

        /*Send Mag,Accel,Gyro Data.
        kw40z_device.SendGyro(x,y,z);
        kw40z_device.SendAccel(z,x,y);
        kw40z_device.SendMag(y,z,x);*/

        Thread::wait(1000);
    }
}
void UpdateSensorData(void)
{
    testsignal+=1;
    /*battery -= 5;
    if(battery < 5) battery = 100;

    light += 20;
    if(light > 100) light = 0;

    humidity += 500;
    if(humidity > 8000) humidity = 2000;

    temperature -= 200;
    if(temperature < 200) temperature = 4200;

    pressure += 300;
    if(pressure > 10300) pressure = 7500;

    x += 1400;
    y -= 2300;
    z += 1700;*/
}

void interruptHandlerQueued()
{
//    int temp_ppg[num_samples];
    for(int iter = 0; iter < num_samples; iter+=0) {
        
        MAX30101::InterruptBitField_u interruptStatus;
        hr.getInterruptStatus(interruptStatus);
//    printf("Interrupt Status: 0x%02x\r\n", interruptStatus.all);

        if (interruptStatus.bits.pwr_rdy == 0x1) {
//        printf("Powered on\r\n");

            // Soft reset
            MAX30101::ModeConfiguration_u modeConf;
            modeConf.all = 0;
            modeConf.bits.reset = 1;
            hr.setModeConfiguration(modeConf);
            wait(0.01);

            // Configure FIFO
            MAX30101::FIFO_Configuration_u fifoConf;
            hr.getFIFOConfiguration(fifoConf);
//        pc.printf("FIFO Configuration: 0x%02x\r\n", fifoConf.all);

            // Set LED power
            hr.setLEDPulseAmplitude(MAX30101::LED1_PA, 0x0C);
            hr.setLEDPulseAmplitude(MAX30101::ProxModeLED_PA, 0x19);
//        pc.printf("LED set\r\n");

            MAX30101::SpO2Configuration_u spo2Conf;
            hr.getSpO2Configuration(spo2Conf);
            spo2Conf.bits.led_pw = MAX30101::PW_1;
            spo2Conf.bits.spo2_sr = MAX30101::SR_100_Hz;
            hr.setSpO2Configuration(spo2Conf);
            hr.getSpO2Configuration(spo2Conf);
//        pc.printf("SpO2 Configuration: 0x%02x\r\n", spo2Conf.all);

            // Proximity settings
            hr.setProxIntThreshold(0x14);

            // Enable HR mode
            modeConf.all = 0;
            modeConf.bits.mode = MAX30101::HeartRateMode;
            hr.setModeConfiguration(modeConf);
//        printf("Mode set\r\n");
        }

        if (interruptStatus.bits.prox_int == 0x1) {
//        printf("Proximity Triggered, entered HR Mode.");
        }

        if (interruptStatus.bits.ppg_rdy == 0x1) {
//        printf("PPG Ready.\r\n");
            mask_ppg = 1;
        }

        if (interruptStatus.bits.a_full == 0x1) {
//        printf("FIFO Almost Full.\r\n");
            uint8_t data[FIFO_DATA_MAX];
            uint16_t readBytes = 0;

            hr.readFIFO(MAX30101::OneLedChannel, data, readBytes);
            printf("data length: %u \r\n",readBytes);
            //printf("data length: %u \r\n",data);
            for (uint16_t i = 0; i < readBytes; i += 3) {
                uint8_t sample[4] = {0};
                sample[0] = data[i + 2];
                sample[1] = data[i + 1];
                sample[2] = data[i];

                num = *(uint32_t *) sample;
                if (num < 310000) {
                    ppg_single_sample = 0;
                    printf("keep closer to your hand \r\n");
                } else {

                    //ppg_single_sample = 65;
                    ppg_single_sample = num;
                    if(iter < num_samples)
                        ppg[iter] = num;
//                    printf("%d\r\n", ppg_single_sample); // I commented this out
                    iter++;
                }
                printf("%d; %d\r\n", iter, ppg[iter-1]);


            }
        }

        interruptStatus.all = 0xFF;
        
        if (mask_ppg == 1) {
            interruptStatus.bits.ppg_rdy = 0;
        }
        hr.enableInterrupts(interruptStatus);
    }
    /*
    for(int i = 0; i < num_samples; i++) {
        ppg[i] = temp_ppg[i];
    }
    */
    //ppg = temp_ppg;
}

void interruptHandler()
{
    evqueue.call(interruptHandlerQueued);
    
    // moving average
    double movave[num_samples];
    int avecap = 25;
    int i = 0;
    int j = 0;
    for(i = 0; i < (avecap-1)/2; i++) {
        movave[i] = ppg[0];
        for(j = 1; j < 1+(avecap-1)/2; j++) {
            movave[i] = movave[i] + ppg[j];
        }
        movave[i] = (double)(movave[i]/(i+(avecap-1)/2-1));
    }
    for(i = (num_samples-(avecap-1)/2); i < num_samples; i++) {
        movave[i] = ppg[i-(avecap-1)/2-1];
        for(j = (i-(avecap-1)/2); j < num_samples; j++) {
            movave[i] = movave[i] + ppg[j];
        }
        movave[i] = (double)(movave[i]/(num_samples-i+(avecap-1)/2));
    }
    for(i = (avecap-1)/2; i < (num_samples-(avecap-1)/2); i++) {
        movave[i] = ppg[i-(avecap-1)/2-1];
        for(j = i-(avecap-1)/2; j < i+(avecap-1)/2; j++) {
            movave[i] = movave[i] + ppg[j];
        }
        movave[i] = (double)(movave[i]/avecap);
    }
    // normalize ppg
    for(i = 0; i < num_samples; i++) {
        ppg[i] = ppg[i] - movave[i];
    }
    
    // smoothing curve
    for(i = 1; i < num_samples; i++) {
        ppg[i] = ppg[i] + ppg[i-1];
    }
    
    // AMPD Algorithm
    const int kcap = 25;
    int m[kcap][num_samples];
    for(int k = 0; k < kcap; k++) {
        for(i = 1; i < num_samples; i++) {
            if(i-1 < 0 || i-k-1 < 0 || i+k-1 >= num_samples)
                m[k][i] = 0;
            else if(ppg[i-1] > ppg[i-k-1] && ppg[i-1] > ppg[i+k-1])
                m[k][i] = 1;
            else
                m[k][i] = 0;
        }
    }
    int max_mult[num_samples];
    for(i = 0; i < num_samples; i++) { // max_mult = m(1,:)';
        max_mult[i] = m[0][i];
    }
    for(int k = 1; k < kcap; k++) {
        for(i = 0; i < num_samples; i++) {
            max_mult[i] = max_mult[i]*m[k][i];
        }
    }
    
    // extract times that are max
    int num_max = 0;
    for(i = 0; i < num_samples; i++) {
        num_max = max_mult[i];
    }
    vector<double> time_of_max;
    vector<int> index_of_max;
    vector<int> max_points;
    for(i = 0; i < num_samples; i++) {
        if(max_mult[i] == 1) {
            time_of_max.push_back(TIME[i-1]);
            index_of_max.push_back(i-1);
            max_points.push_back(ppg[i-1]);
        }
    } 
}

// main() runs in its own thread in the OS
int main()
{
//    printf("Hello world.\r\n");
    for(int i = 0; i < num_samples; i++) {
        TIME[i] = (double)(i*(30.0/num_samples));
    }
    
    t.start(callback(&evqueue, &EventQueue::dispatch_forever));
    kw40z_device.attach_buttonLeft(&ButtonLeft);
    kw40z_device.attach_buttonRight(&ButtonRight);

    pwr1v8 = 1;
    pwr3v3b = 1;
    pwr15v = 0;

    maximInterrupt.fall(interruptHandler);
    maximInterrupt.enable_irq();

    MAX30101::InterruptBitField_u interruptStatus;
    interruptStatus.all = 0xFF;
    hr.enableInterrupts(interruptStatus);

    char text[20];  /* Text Buffer */
    oled_text_properties_t textProperties = {0};
    oled.GetTextProperties(&textProperties);
    /* Turn on the backlight of the OLED Display */
    oled.DimScreenON();

    /* Fills the screen with solid black */
    oled.FillScreen(COLOR_BLACK);
    strcpy((char *) text, "Raw PPG:");
    oled.Label((uint8_t *)text,7,0);

    strcpy((char *) text, "SDNN; LF/HF:");
    oled.Label((uint8_t *)text,7,40);
    //dynamic text setup
    textProperties.fontColor = COLOR_WHITE;
    textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
    oled.SetTextProperties(&textProperties);

    txThread.start(txTask);
    while (true) {

        /* Format the time reading */
        sprintf(text,"%d",ppg_single_sample);

        /* Display time reading in 35px by 15px textbox at(x=55, y=40) */
        oled.TextBox((uint8_t *)text,55,15,35,15); //Increase textbox for more digits
        if (ppg_single_sample > 45) {
            sprintf(text,"%d; %.2f",SDNN, HF_LF);

            /* Display time reading in 35px by 15px textbox at(x=55, y=40) */
            oled.TextBox((uint8_t *)text,55,55,35,15); //Increase textbox for more digits
        } else {
            sprintf(text,"wait HR");

            /* Display time reading in 35px by 15px textbox at(x=55, y=40) */
            oled.TextBox((uint8_t *)text,55,55,35,15);
        }

        Thread::wait(1000);
    }
    return 0;
}