File content as of revision 4:eb89733b8642:

#include "mbed.h"
#include "mbed_events.h"
#include "MAX30101.h"
#include "Hexi_KW40Z.h"
#include "Hexi_OLED_SSD1351.h"
#include "OLED_types.h"
#include "OpenSans_Font.h"
#include "string.h"
#include "FXOS8700.h"


#define FIFO_DATA_MAX 288



#define LED_ON      0
#define LED_OFF     1

void UpdateSensorData(void);
void StartHaptic(void);
void StopHaptic(void const *n);
void txTask(void);

DigitalOut led1(LED_GREEN); // RGB LED
FXOS8700 accel(PTC11, PTC10);

DigitalOut pwr1v8(PTA29);
DigitalOut pwr3v3b(PTC13);
DigitalOut pwr15v(PTB12);
I2C i2c0(PTB1, PTB0);
InterruptIn maximInterrupt(PTB18);
Serial pc(USBTX, USBRX);

DigitalOut redLed(LED1,1);
DigitalOut greenLed(LED2,1);
DigitalOut blueLed(LED3,1);
DigitalOut haptic(PTB9);


RtosTimer hapticTimer(StopHaptic, osTimerOnce);

/* Instantiate the Hexi KW40Z Driver (UART TX, UART RX) */ 
KW40Z kw40z_device(PTE24, PTE25);

/* Instantiate the SSD1351 OLED Driver */ 
SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); /* (MOSI,SCLK,POWER,CS,RST,DC) */

/*Create a Thread to handle sending BLE Sensor Data */ 
Thread txThread;

EventQueue evqueue(32 * EVENTS_EVENT_SIZE);
Thread t;

MAX30101 hr(i2c0);

int mask_ppg = 0;
uint32_t count = 0;



char text[20]; 

uint8_t battery = 100;
uint8_t light = 0;
uint16_t humidity = 4500;
uint16_t temperature = 2000;
uint16_t pressure = 9000;
uint16_t x = 0;
uint16_t y = 5000;
uint16_t z = 10000;

// Variables
float accel_data[3]; // Storage for the data from the sensor
float accel_rms=0.0; // RMS value from the sensor
float ax, ay, az; // Integer value from the sensor to be displayed
const uint8_t *image1; // Pointer for the image1 to be displayed
char text1[20]; // Text Buffer for dynamic value displayed
char text2[20]; // Text Buffer for dynamic value displayed
char text3[20]; // Text Buffer for dynamic value displayed
float dot;
float old_acc=0;
float new_acc=0;
float old_accx, old_accy, old_accz, old_dot=0.0;
uint8_t StepNum = 0, StepNumber = 0;

float filter_buf[75];

/****************************Call Back Functions*******************************/

float Filter(int s) 
{
    accel.acquire_accel_data_g(accel_data);
    float filter_sum = 0.0;
    //printf("%d\n\r",s);
    for(int i = 0; i < 75; i++) 
    {
    filter_buf[i] = accel_data[s];
    //printf("%4.2f\n\r",filter_buf[i]);
    filter_sum += filter_buf[i];
    }
    return (float)(filter_sum / 75);
}


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

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

void ButtonUp(void)
{
    StartHaptic();
    oled.FillScreen(COLOR_BLACK);

    /* Get OLED Class Default Text Properties */
    oled_text_properties_t textProperties = {0};
    oled.GetTextProperties(&textProperties);    
        
    /* Change font color to Blue */ 
    textProperties.fontColor   = COLOR_BLUE;
    oled.SetTextProperties(&textProperties);
    strcpy((char *) text1,"Steps: ");
    oled.Label((uint8_t *)text1,3,45);      
    sprintf(text1,"%d",StepNumber);
      /* Display time reading in 35px by 15px textbox at(x=55, y=40) */
    oled.TextBox((uint8_t *)text1,70,45,20,15); //Increase textbox for more digits
    
    
}
void ButtonDown(void)
{
    oled.FillScreen(COLOR_BLACK);

    /* Get OLED Class Default Text Properties */
    oled_text_properties_t textProperties = {0};
    oled.GetTextProperties(&textProperties);    
        
    /* Change font color to Blue */ 
    textProperties.fontColor   = COLOR_BLUE;
    oled.SetTextProperties(&textProperties);
    
    /* Display Bluetooth Label at x=17,y=65 */ 
    strcpy((char *) text,"BLUETOOTH");
    oled.Label((uint8_t *)text,17,65);
    
    /* Change font color to white */ 
    textProperties.fontColor   = COLOR_WHITE;
    textProperties.alignParam = OLED_TEXT_ALIGN_CENTER;
    oled.SetTextProperties(&textProperties);
    
    /* Display Label at x=22,y=80 */ 
    strcpy((char *) text,"Tap Below");
    oled.Label((uint8_t *)text,22,80);
    
    
}
void PassKey(void)
{
    StartHaptic();
    strcpy((char *) text,"PAIR CODE");
    oled.TextBox((uint8_t *)text,0,25,95,18);
  
    /* Display Bond Pass Key in a 95px by 18px textbox at x=0,y=40 */
    sprintf(text,"%d", kw40z_device.GetPassKey());
    oled.TextBox((uint8_t *)text,0,40,95,18);
}

// Key modification: use the alert functionality enabled by the host-ble interface
// to define our own command.
void AlertReceived(uint8_t *data, uint8_t length)
{
    StartHaptic();
    data[19] = 0;
    pc.printf("%s\n\r", data);
    
    // data (our command) must 20 bytes long.
    // CMD for turning on: 'ledonledonledonledon'
    if (data[4] == 'n') {
        greenLed = LED_ON;
        redLed = LED_ON;
        blueLed = LED_ON;
        pc.printf("on\n\r", data);
    
    // CMD for turning off: 'ledoffledoffledoffled'
    } else if (data[4] == 'f') {
        greenLed = LED_OFF;
        redLed = LED_OFF;
        blueLed = LED_OFF;
        pc.printf("off\n\r", data);
    }
}
void interruptHandlerQueued() {
    
    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);
        
        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];
            
        printf("%u\r\n", *(uint32_t *) sample);
        }
    }
    
    interruptStatus.all = 0xFF;
    if (mask_ppg == 1) {
        interruptStatus.bits.ppg_rdy = 0;
    }
    hr.enableInterrupts(interruptStatus);
}

void interruptHandler() {
    evqueue.call(interruptHandlerQueued);
}

// main() runs in its own thread in the OS
int main() {
//    printf("Hello world.\r\n");
    
    t.start(callback(&evqueue, &EventQueue::dispatch_forever));
      accel.accel_config();
    /* Register callbacks to application functions */
    kw40z_device.attach_buttonLeft(&ButtonLeft);
    kw40z_device.attach_buttonRight(&ButtonRight);
    kw40z_device.attach_buttonDown(&ButtonDown);
    kw40z_device.attach_buttonUp(&ButtonUp);
    kw40z_device.attach_passkey(&PassKey);
    kw40z_device.attach_alert(&AlertReceived);

    pc.printf("hello\n\r");
    
    /* Turn on the backlight of the OLED Display */
    oled.DimScreenON();
    
    /* Fills the screen with solid black */         
    
         
    uint8_t prevLinkState = 0; 
    uint8_t currLinkState = 0;
    
    
    pwr1v8 = 1;
    pwr3v3b = 1;
    pwr15v = 0;
    
    maximInterrupt.fall(interruptHandler);
    maximInterrupt.enable_irq();
    
    MAX30101::InterruptBitField_u interruptStatus;
    interruptStatus.all = 0xFF;
    hr.enableInterrupts(interruptStatus);
    
    
     txThread.start(txTask); /*Start transmitting Sensor Tag Data */
    
    while (true) 
    {
        // blueLed = !kw40z_device.GetAdvertisementMode(); /*Indicate BLE Advertisment Mode*/   
        Thread::wait(50);
    }
     
    return 0;
}
void txTask(void){
   
   while (true) 
   {
        UpdateSensorData();
        
        /*Notify Hexiwear App that it is running Sensor Tag mode*/
        kw40z_device.SendSetApplicationMode(GUI_CURRENT_APP_SENSOR_TAG);
                
        /*The following is sending dummy data over BLE. Replace with real data*/
    
        /*Send Battery Level for 20% */ 
        kw40z_device.SendBatteryLevel(StepNumber);
               
        /*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(10);                 
    }
}

void UpdateSensorData(void)
{    
        accel.acquire_accel_data_g(accel_data);
      //printf("Accelerometer \tX-Axis %4.2f \tY-Axis %4.2f \tZ-Axis %4.2f \tRMS %4.2f\n\r",accel_data[0],accel_data[1],accel_data[2],accel_rms);
      ax = Filter(0);
      ay = Filter(1);
      az = Filter(2);  
      wait(0.02);           
      accel_rms = sqrt((ax*ax)+(ay*ay)+(az*az)/3);
      //printf("Accelerometer \tX-Axis %4.2f \tY-Axis %4.2f \tZ-Axis %4.2f \tRMS %4.2f\n\r",ax,ay,az,accel_rms);
      dot = (old_accx * ax)+(old_accy * ay)+(old_accz * az);
      old_acc = abs(sqrt(old_accx*old_accx+old_accy*old_accy+old_accz*old_accz));
      new_acc = abs(sqrt(ax*ax+ay*ay+az*az));
      //printf("\nOld Acceleration: %4.2f\n\r",old_acc);
      //printf("New Acceleration: %4.2f\n\r",new_acc);
      dot /= (old_acc * new_acc);
      //printf("\nDot: %4.2f\n\r",dot);
      //printf("Old Dot: %4.2f\n\r",old_dot);
      
       oled_text_properties_t textProperties = {0};
      oled.GetTextProperties(&textProperties); 
      
      /* Set text properties to white and right aligned for the dynamic text */
      textProperties.fontColor = COLOR_BLUE;
      textProperties.alignParam = OLED_TEXT_ALIGN_LEFT;
      oled.SetTextProperties(&textProperties);  
      
      /* Display Legends */
      //strcpy((char *) text1,"Steps: ");
      //oled.Label((uint8_t *)text1,3,45);      
      StepNum = StepNumber;
      if(abs(dot - old_dot) >= 0.05 && abs(dot - old_dot) <= 0.10) 
      {
            StepNumber += 1;
        
      }
      //printf("%4.2f\n\r",dot);
      old_accx = ax;
      old_accy = ay;
      old_accz = az;
      old_dot = dot;
      
        Thread::wait(250);
}


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

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