File content as of revision 7:2c9c8b6a28a7:

/*******************************************************************************
* Copyright (C) 2018 Maxim Integrated Products, Inc., All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included
* in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
* IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
* OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*
* Except as contained in this notice, the name of Maxim Integrated
* Products, Inc. shall not be used except as stated in the Maxim Integrated
* Products, Inc. Branding Policy.
*
* The mere transfer of this software does not imply any licenses
* of trade secrets, proprietary technology, copyrights, patents,
* trademarks, maskwork rights, or any other form of intellectual
* property whatsoever. Maxim Integrated Products, Inc. retains all
* ownership rights.
*******************************************************************************
*/
#include <config.h>
#include <mbed.h>
#include <events/mbed_events.h>
#include <rtos.h>
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "max32630fthr.h"

#if defined(LIB_MAX30003)
#include "MAX30003.h"
#endif

#if defined(LIB_MAX30205)
#include "MAX30205.h"
#endif

#if defined(LIB_MAX30101)
#include "MAX30101.h"
#include "max30101_algo.h"
#endif

#if defined(LIB_MAX113XX_PIXI)
#include "MAX113XX_Pixi.h"
#include "MAX11301Hex.h"
#endif

/******************************************************************************/

MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);

InterruptIn button(P2_3);

SPI spim2(SPI2_MOSI, SPI2_MISO, SPI2_SCK);

I2C i2c1(I2C1_SDA, I2C1_SCL);		/* I2C bus, P3_4 = SDA, P3_5 = SCL */

/* LEDs */
DigitalOut rLED(LED1, LED_OFF);
DigitalOut gLED(LED2, LED_OFF);
DigitalOut bLED(LED3, LED_OFF);

/* Hardware serial port over DAPLink */
Serial daplink(USBTX, USBRX, 115200);

int aliveLedEventId;

/******************************************************************************/
const static char     DEVICE_NAME[] = MAXIM_PLATFORM_NAME;
static const uint16_t uuid16_list[] = {0xFFFF}; //Custom UUID, FFFF is reserved for development

/* Set Up custom Characteristics */
UUID iotServiceUUID  ("00001520-1d66-11e8-b467-0ed5f89f718b");

UUID uuidButtonPressedNotify("00001522-1d66-11e8-b467-0ed5f89f718b");
static uint8_t buttonPressedCount = 0;
GattCharacteristic gattCharButtonPressedNotify(uuidButtonPressedNotify, &buttonPressedCount, 1, 1,
							  	  	  	    GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

UUID uuidRGBLED("00001523-1d66-11e8-b467-0ed5f89f718b");
static uint8_t RGBLedInitValue[] = {LED_OFF, LED_OFF, LED_OFF};
ReadWriteArrayGattCharacteristic<uint8_t, sizeof(RGBLedInitValue)> gattCharRGBLed(uuidRGBLED, RGBLedInitValue);

#if defined(LIB_MAX30003_ECG)
UUID uuidECG("00001524-1d66-11e8-b467-0ed5f89f718b");
static int16_t ECGInitValue = 0xABCD;
ReadOnlyGattCharacteristic<int16_t> gattCharECG(uuidECG, &ECGInitValue,
									GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
#else
UUID uuidBPM("00001524-1d66-11e8-b467-0ed5f89f718b");
static float BPMInitValue = 0.0;
ReadOnlyGattCharacteristic<float> gattCharBPM(uuidBPM, &BPMInitValue,
									GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
#endif

#if defined(LIB_MAX30101)
UUID uuidHeartRate("00001525-1d66-11e8-b467-0ed5f89f718b");
static uint16_t HeartRateInitValue = 0xEEFF;
ReadOnlyGattCharacteristic<uint16_t> gattCharHeartRate(uuidHeartRate, &HeartRateInitValue,
									 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

UUID uuidSPO2("00001526-1d66-11e8-b467-0ed5f89f718b");
static uint16_t SPO2InitValue = 0xAABB;
ReadOnlyGattCharacteristic<uint16_t> gattCharSPO2(uuidSPO2, &SPO2InitValue,
									 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
#endif

#if defined(LIB_MAX113XX_PIXI)
UUID uuidADC("00001527-1d66-11e8-b467-0ed5f89f718b");
static float ADCInitValue = 0;
ReadOnlyGattCharacteristic<float> gattCharADC(uuidADC, &ADCInitValue,
									 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
#endif

#if defined(LIB_MAX30205)
UUID uuidTemp("00001528-1d66-11e8-b467-0ed5f89f718b");
static float TempInitValue = 26.5;
ReadOnlyGattCharacteristic<float> gattCharTemp(uuidTemp, &TempInitValue,
								  GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
#endif

/* Set up custom service */
GattCharacteristic *characteristics[] = {&gattCharRGBLed, &gattCharButtonPressedNotify,
#if defined(LIB_MAX30003_ECG)
										 &gattCharECG,
#else
										 &gattCharBPM,
#endif
#if defined(LIB_MAX30205)
										 &gattCharTemp,
#endif
#if defined(LIB_MAX30101)
										 &gattCharHeartRate,
										 &gattCharSPO2,
#endif
#if defined(LIB_MAX113XX_PIXI)
										 &gattCharADC,
#endif
};

GattService iotService(iotServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));

/******************************************************************************/

Mutex ble_mutex;

static EventQueue eventQueue(/* event count */ 10 * /* event size */ 32);

ble_error_t bleGattAttrWrite(GattAttribute::Handle_t handle, const uint8_t *value, uint16_t size)
{
	BLE &ble = BLE::Instance();
	ble_error_t ret;

	ble_mutex.lock();

	ret = ble.gattServer().write(handle, value, size);

	ble_mutex.unlock();

	return ret;
}

void updateButtonState(uint8_t newState) {
	printf("Button pressed...\r\n");
	bleGattAttrWrite(gattCharButtonPressedNotify.getValueHandle(), (uint8_t *)&newState, sizeof(uint8_t));
}

void buttonPressedCallback(void)
{
    eventQueue.call(Callback<void(uint8_t)>(&updateButtonState), ++buttonPressedCount);
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
	printf("disc\r\n");
    BLE::Instance().gap().startAdvertising(); // restart advertising
}

/* Connection */
void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
	printf("succ\r\n");
}

void blinkCallback(void)
{
	gLED = !gLED;
}

void onBleInitError(BLE &ble, ble_error_t error)
{
    /* Initialization error handling should go here */
}

/**
 * This callback allows the LEDService to receive updates to the ledState Characteristic.
 *
 * @param[in] params
 *     Information about the characteristic being updated.
 */
void onDataWrittenCallback(const GattWriteCallbackParams *params)
{
	if ((params->handle == gattCharRGBLed.getValueHandle()) && (params->len >= 3)) {
    	rLED = (params->data[0] != 0) ? LED_OFF : LED_ON;
    	gLED = (params->data[1] != 0) ? LED_OFF : LED_ON;
    	bLED = (params->data[2] != 0) ? LED_OFF : LED_ON;
    }
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }

    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(connectionCallback);

    ble.gattServer().onDataWritten(onDataWrittenCallback);

    ble.gattServer().addService(iotService);

    /* setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
    ble.gap().startAdvertising();

    button.fall(buttonPressedCallback);
}

void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
    BLE &ble = BLE::Instance();
    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}

/******************************************************************************
 ************** MAX30205EVSYS *************************************************
 ******************************************************************************/
#if defined(LIB_MAX30205)

#define MAX30205_DATA_READ_PERIOD_MSEC	5000

MAX30205 max30205_temp_sensor(i2c1, 0x48);	/* New MAX30205 on i2cBus */

Thread thread_max30205_reader;

bool max30205_config(MAX30205 &temp_sensor){

    int rc = 0;

    MAX30205::Configuration_u temp_cfg;
    temp_cfg.all = 0;
    temp_cfg.bits.shutdown = 1;     // Shutdown mode
    temp_cfg.bits.comp_int = 1;     // Interrupt mode
    temp_cfg.bits.os_polarity = 0;  // Active low OS
    temp_cfg.bits.fault_queue = 1;  // Two faults for OS condition
    temp_cfg.bits.data_format = 0;  // Normal data format
    temp_cfg.bits.timeout = 0;      // I2C timeout reset enabled
    temp_cfg.bits.one_shot = 0;     // Start with one-shot = 0

    rc = temp_sensor.writeConfiguration(temp_cfg);  // Write config to MAX30205

    return rc;
}

void max30205_reader_task()
{
	int rc = max30205_config(max30205_temp_sensor);   // Configure sensor, return 0 on success

	MAX30205::Configuration_u temp_cfg;
	uint16_t rawTemperatureRead;
	float temperature;

	temp_cfg.all = 0;

	daplink.printf("Starting MAX30205 Temperature Demo Application...\r\n");

	while (1) {
		if (rc == 0) {
			/* Send one-shot cmd to begin conversion */
			temp_cfg.bits.one_shot = 1;
			rc = max30205_temp_sensor.writeConfiguration(temp_cfg);

			Thread::wait(50);

			/* Read the temperature data */
			rc = max30205_temp_sensor.readTemperature(rawTemperatureRead);

			/* Convert temp data to Celsius */
			temperature = max30205_temp_sensor.toCelsius(rawTemperatureRead);

			bleGattAttrWrite(gattCharTemp.getValueHandle(), (uint8_t *)&temperature, sizeof(temperature));
			daplink.printf("Temperature is %2.3f deg. C\r\n", temperature);

			Thread::wait(MAX30205_BLE_NOTIFY_PERIOD_SEC * 1000);

		} else {
			daplink.printf("Something went wrong, check the I2C bus and power connections...\r\n");

			return;
		}
	}
}
#endif

/******************************************************************************
 ************** MAX30101WING **************************************************
 ******************************************************************************/
#if defined(LIB_MAX30101)
#define MAX30101_IRQ_ASSERTED_ID	1

//variable for the algorithm
uint16_t sampleRate =100;
uint16_t compSpO2=1;
int16_t ir_ac_comp =0;
int16_t red_ac_comp=0;
int16_t green_ac_comp=0;
int16_t ir_ac_mag=0;
int16_t red_ac_mag=0;
int16_t green_ac_mag=0;
uint16_t HRbpm2=0;
uint16_t SpO2B=0;
uint16_t DRdy=0;

//declare large variables outside of main
uint32_t redData[500];//set array to max fifo size
uint32_t irData[500];//set array to max fifo size
uint32_t greenData[500];//set array to max fifo size

Thread thread_max30101_reader;

bool max30101_config(MAX30101 &op_sensor)
{

    //Reset Device
    MAX30101::ModeConfiguration_u modeConfig;
    modeConfig.all = 0;
    modeConfig.bits.reset = 1;
    modeConfig.bits.mode = MAX30101::MultiLedMode;     // Sets SPO2 Mode
    int32_t rc = op_sensor.setModeConfiguration(modeConfig);

    //enable MAX30101 interrupts
    MAX30101::InterruptBitField_u ints;
    if(rc == 0) {
        ints.all = 0;
        ints.bits.a_full = 1;       // Enable FIFO almost full interrupt
        ints.bits.ppg_rdy =1;       //Enables an interrupt when a new sample is ready
        rc = op_sensor.enableInterrupts(ints);
    }

    //configure FIFO
    MAX30101::FIFO_Configuration_u fifoConfig;
    if(rc == 0) {
        fifoConfig.all = 0;
        fifoConfig.bits.fifo_a_full = 10;                            // Max level of 17 samples
        fifoConfig.bits.sample_average = MAX30101::AveragedSamples_0;// Average 0 samples
        rc = op_sensor.setFIFOConfiguration(fifoConfig);
    }

    MAX30101::SpO2Configuration_u spo2Config;
    if(rc == 0) {
        spo2Config.all = 0;                                 // clears register
        spo2Config.bits.spo2_adc_range = 1;                 //sets resolution to 4096 nAfs
        spo2Config.bits.spo2_sr = MAX30101::SR_100_Hz;     // SpO2 SR = 100Hz
        spo2Config.bits.led_pw = MAX30101::PW_3;            // 18-bit ADC resolution ~400us
        rc = op_sensor.setSpO2Configuration(spo2Config);
    }

    //Set time slots for LEDS
    MAX30101::ModeControlReg_u multiLED;
    if (rc == 0) {
        //sets timing for control register 1
        multiLED.bits.lo_slot=1;
        multiLED.bits.hi_slot=2;
        rc = op_sensor.setMultiLEDModeControl(MAX30101::ModeControlReg1, multiLED);
        if (rc == 0) {
            multiLED.bits.lo_slot=3;
            multiLED.bits.hi_slot=0;
            rc = op_sensor.setMultiLEDModeControl(MAX30101::ModeControlReg2, multiLED);
        }
    }

    //Set LED drive currents
    if(rc == 0) {
        // Heart Rate only, 1 LED channel, Pulse amp. = ~7mA
        rc = op_sensor.setLEDPulseAmplitude(MAX30101::LED1_PA, 0x24);
        //To include SPO2, 2 LED channel, Pulse amp. ~7mA
        if (rc == 0) {
            rc = op_sensor.setLEDPulseAmplitude(MAX30101::LED2_PA, 0x24);
        }
        if (rc == 0) {
            rc = op_sensor.setLEDPulseAmplitude(MAX30101::LED3_PA, 0x24);
        }

    }

    //Set operating mode
    modeConfig.all = 0;
    if(rc == 0) {
        modeConfig.bits.mode = MAX30101::MultiLedMode;     // Sets multiLED mode
        rc = op_sensor.setModeConfiguration(modeConfig);
    }


    return rc;
}

void max30101wing_pmic_config(I2C & i2c_bus, DigitalOut & pmic_en)
{

    const uint8_t PMIC_ADRS = 0x54;
    const uint8_t BBB_EXTRA_ADRS = 0x1C;
    const uint8_t BOOST_VOLTAGE = 0x05;

    char data_buff[] = {BBB_EXTRA_ADRS, 0x40};    //BBBExtra register address
    //and data to enable passive
    //pull down.
    i2c_bus.write(PMIC_ADRS, data_buff,2);        //write to BBBExtra register

    data_buff[0] = BOOST_VOLTAGE;
    data_buff[1] = 0x08;                          //Boost voltage configuration
    //register followed by data
    //to set voltage to 4.5V 1f
    pmic_en = 0;                                  //disables VLED 08
    i2c_bus.write(PMIC_ADRS, data_buff,2);        //write to BBBExtra register
    pmic_en = 1;                                  //enables VLED
}

/* Op Sensor FIFO nearly full callback */
void max30101_intr_callback()
{
	thread_max30101_reader.signal_set(MAX30101_IRQ_ASSERTED_ID);
}

void max30101_reader_task()
{
	InterruptIn op_sensor_int(P3_2);				// Config P3_2 as int. in for
	op_sensor_int.fall(max30101_intr_callback);		// FIFO ready interrupt

	DigitalOut VLED_EN(P3_3,0);						//Enable for VLEDs
	max30101wing_pmic_config(i2c1, VLED_EN);

	MAX30101 op_sensor(i2c1);						// Create new MAX30101 on i2cBus
	int rc = max30101_config(op_sensor);			// Config sensor, return 0 on success

	MAX30101::InterruptBitField_u ints;				// Read interrupt status to clear
	rc = op_sensor.getInterruptStatus(ints);		// power on interrupt

	uint8_t fifoData[MAX30101::MAX_FIFO_BYTES];
	uint16_t idx, readBytes;
	int32_t opSample;
	uint32_t sample;
	uint16_t HRTemp;
	uint16_t spo2Temp;

	uint16_t lastValidHR = 0;
	uint16_t lastValidSPO2 = 0;

	int r=0; //counter for redData position
	int ir=0; //counter for irData position
	int g =0; //counter for greenData position
	int c=0; //counter to print values

	daplink.printf("Starting MAX30101 HeartRate / SPO2 Demo Application...\r\n");
	daplink.printf("Please wait a few seconds while data is being collected.\r\n");

    Timer bleNotifyTimer;

    bleNotifyTimer.start();

	while (1) {
		if (rc == 0) {
			/* Check if op_sensor interrupt asserted */
			Thread::signal_wait(MAX30101_IRQ_ASSERTED_ID);

			/* Read interrupt status to clear interrupt */
			rc = op_sensor.getInterruptStatus(ints);

			/* Confirms proper read prior to executing */
			if (rc == 0) {
				// Read FIFO
				rc = op_sensor.readFIFO(MAX30101::ThreeLedChannels, fifoData, readBytes);

				if (rc == 0) {
					/* Convert read bytes into samples */
					for (idx = 0; idx < readBytes; idx+=9) {
						if (r >= 500 || ir >= 500 || g >= 500) {
							daplink.printf("Overflow! r=%d ir=%d g=%d\r\n", r, ir, g);
							break;
						}

						if (readBytes >= (idx + 8)) {
							redData[r++] = ((fifoData[idx] << 16) | (fifoData[idx + 1] << 8) | (fifoData[idx + 2])) & 0x03FFFF;
							irData[ir++] = ((fifoData[idx + 3] << 16) | (fifoData[idx + 4] << 8) | (fifoData[idx + 5])) & 0x03FFFF;
							greenData[g++] = ((fifoData[idx + 6] << 16) | (fifoData[idx + 7] << 8) | (fifoData[idx + 8])) & 0x03FFFF;
						}
					}

					if ((r >= 500) && (ir >= 500) && (g >= 500)) {/* checks to make sure there are 500 */
						/* samples in data buffers */

						/* runs the heart rate and SpO2 algorithm */
						for (c = 0, HRTemp = 0; c < r; c++) {
							HRSpO2Func(irData[c], redData[c],greenData[c], c,sampleRate, compSpO2,
									   &ir_ac_comp,&red_ac_comp, &green_ac_comp, &ir_ac_mag,&red_ac_mag,
									   &green_ac_mag, &HRbpm2,&SpO2B,&DRdy);
							if (DRdy) {
								HRTemp = HRbpm2;
								spo2Temp = SpO2B;
							}
						}

						/* If the above algorithm returns a valid heart rate on the last sample, it is printed */
						if (DRdy == 1) {
							daplink.printf("Heart Rate = %i\r\n",HRbpm2);
							daplink.printf("SPO2 = %i\r\n",SpO2B);
							lastValidHR = HRbpm2;
							lastValidSPO2 = SpO2B;
						} else if (HRTemp != 0) { /* if a valid heart was calculated at all, it is printed */
							daplink.printf("Heart Rate = %i\r\n",HRTemp);
							daplink.printf("SPO2 = %i\r\n",spo2Temp);
							lastValidHR = HRTemp;
							lastValidSPO2 = spo2Temp;
						} else {
							daplink.printf("Calculation failed...waiting for more samples...\r\n");
							daplink.printf("Please keep your finger on the MAX30101 sensor with minimal movement.\r\n");
						}

						/* dump the first hundred samples after calculation */
						for (c = 100; c < 500; c++) {
							redData[c - 100] = redData[c];
							irData[c - 100] = irData[c];
							greenData[c - 100] = greenData[c];
						}

						/* reset counters */
						r = 400;
						ir = 400;
						g = 400;
					}

					if (bleNotifyTimer.read_ms() >= (MAX30101_BLE_NOTIFY_PERIOD_SEC * 1000)) {
						bleGattAttrWrite(gattCharHeartRate.getValueHandle(), (uint8_t *)&lastValidHR, sizeof(lastValidHR));
						bleGattAttrWrite(gattCharSPO2.getValueHandle(), (uint8_t *)&lastValidSPO2, sizeof(lastValidSPO2));
						bleNotifyTimer.reset();
					}
				}
			}
		} else { // If rc != 0, a communication error has occurred

			daplink.printf("Something went wrong, "
					  "check the I2C bus or power connections... \r\n");

			return;
		}

	}
}
#endif

/******************************************************************************
************** MAX30003WING (ECG) *********************************************
 ******************************************************************************/
#if defined(LIB_MAX30003_ECG)
#define MAX30003_IRQ_ASSERTED_SIGNAL_ID	1

MAX30003 max30003(spim2, SPI2_SS);		/* MAX30003WING board */

Thread thread_max30003_reader;

void ecg_config(MAX30003& ecgAFE) {

    // Reset ECG to clear registers
    ecgAFE.writeRegister( MAX30003::SW_RST , 0);

    // General config register setting
    MAX30003::GeneralConfiguration_u CNFG_GEN_r;
    CNFG_GEN_r.bits.en_ecg = 1;     // Enable ECG channel
    CNFG_GEN_r.bits.rbiasn = 1;     // Enable resistive bias on negative input
    CNFG_GEN_r.bits.rbiasp = 1;     // Enable resistive bias on positive input
    CNFG_GEN_r.bits.en_rbias = 1;   // Enable resistive bias
    CNFG_GEN_r.bits.imag = 2;       // Current magnitude = 10nA
    CNFG_GEN_r.bits.en_dcloff = 1;  // Enable DC lead-off detection
    ecgAFE.writeRegister( MAX30003::CNFG_GEN , CNFG_GEN_r.all);


    // ECG Config register setting
    MAX30003::ECGConfiguration_u CNFG_ECG_r;
    CNFG_ECG_r.bits.dlpf = 1;       // Digital LPF cutoff = 40Hz
    CNFG_ECG_r.bits.dhpf = 1;       // Digital HPF cutoff = 0.5Hz
    CNFG_ECG_r.bits.gain = 3;       // ECG gain = 160V/V
    CNFG_ECG_r.bits.rate = 2;       // Sample rate = 128 sps
    ecgAFE.writeRegister( MAX30003::CNFG_ECG , CNFG_ECG_r.all);


    //R-to-R configuration
    MAX30003::RtoR1Configuration_u CNFG_RTOR_r;
    CNFG_RTOR_r.bits.en_rtor = 1;           // Enable R-to-R detection
    ecgAFE.writeRegister( MAX30003::CNFG_RTOR1 , CNFG_RTOR_r.all);


    //Manage interrupts register setting
    MAX30003::ManageInterrupts_u MNG_INT_r;
    MNG_INT_r.bits.efit = 0b00011;          // Assert EINT w/ 4 unread samples
    MNG_INT_r.bits.clr_rrint = 0b01;        // Clear R-to-R on RTOR reg. read back
    ecgAFE.writeRegister( MAX30003::MNGR_INT , MNG_INT_r.all);


    //Enable interrupts register setting
    MAX30003::EnableInterrupts_u EN_INT_r;
    EN_INT_r.all = 0;
    EN_INT_r.bits.en_eint = 1;              // Enable EINT interrupt
    EN_INT_r.bits.en_rrint = 0;             // Disable R-to-R interrupt
    EN_INT_r.bits.intb_type = 3;            // Open-drain NMOS with internal pullup
    ecgAFE.writeRegister( MAX30003::EN_INT , EN_INT_r.all);


    //Dyanmic modes config
    MAX30003::ManageDynamicModes_u MNG_DYN_r;
    MNG_DYN_r.bits.fast = 0;                // Fast recovery mode disabled
    ecgAFE.writeRegister( MAX30003::MNGR_DYN , MNG_DYN_r.all);

    // MUX Config
    MAX30003::MuxConfiguration_u CNFG_MUX_r;
    CNFG_MUX_r.bits.openn = 0;          // Connect ECGN to AFE channel
    CNFG_MUX_r.bits.openp = 0;          // Connect ECGP to AFE channel
    ecgAFE.writeRegister( MAX30003::CNFG_EMUX , CNFG_MUX_r.all);

    return;
}

/* ECG FIFO nearly full callback */
//volatile bool ecgFIFOIntFlag = 0;
void ecgFIFO_callback()  {

	thread_max30003_reader.signal_set(MAX30003_IRQ_ASSERTED_SIGNAL_ID);
    //ecgFIFOIntFlag = 1;

}

void max30003_reader_task()
{
	// Constants
	const int EINT_STATUS_MASK =  1 << 23;
	const int FIFO_OVF_MASK =  0x7;
	const int FIFO_VALID_SAMPLE_MASK =  0x0;
	const int FIFO_FAST_SAMPLE_MASK =  0x1;
	const int ETAG_BITS_MASK = 0x7;

	InterruptIn ecgFIFO_int(P5_4);					// Config P5_4 as int. in for the
	ecgFIFO_int.fall(&ecgFIFO_callback);			// ecg FIFO almost full interrupt

	SPI spiBus(SPI2_MOSI, SPI2_MISO, SPI2_SCK);		// SPI bus, P5_1 = MOSI,
													// P5_2 = MISO, P5_0 = SCK

	MAX30003 ecgAFE(spiBus, P5_3);					// New MAX30003 on spiBus, CS = P5_3
	ecg_config(ecgAFE);								// Config ECG


	ecgAFE.writeRegister( MAX30003::SYNCH , 0);

	uint32_t ecgFIFO, readECGSamples, idx, ETAG[32], status;
	int16_t ecgSample[32];

	daplink.printf("Starting MAX30003 ECG Demo Application...\r\n");

	while (1) {

		// Read back ECG samples from the FIFO
		thread_max30003_reader.signal_wait(MAX30003_IRQ_ASSERTED_SIGNAL_ID);

		status = ecgAFE.readRegister( MAX30003::STATUS );      // Read the STATUS register

		// Check if EINT interrupt asserted
		if ( ( status & EINT_STATUS_MASK ) == EINT_STATUS_MASK ) {

			readECGSamples = 0;                        // Reset sample counter

			do {
				ecgFIFO = ecgAFE.readRegister( MAX30003::ECG_FIFO );       // Read FIFO
				ecgSample[readECGSamples] = ecgFIFO >> 8;                  // Isolate voltage data
				ETAG[readECGSamples] = ( ecgFIFO >> 3 ) & ETAG_BITS_MASK;  // Isolate ETAG
				readECGSamples++;                                          // Increment sample counter

			// Check that sample is not last sample in FIFO
			} while ( ETAG[readECGSamples-1] == FIFO_VALID_SAMPLE_MASK ||
					  ETAG[readECGSamples-1] == FIFO_FAST_SAMPLE_MASK );

			// Check if FIFO has overflowed
			if( ETAG[readECGSamples - 1] == FIFO_OVF_MASK ){
				ecgAFE.writeRegister( MAX30003::FIFO_RST , 0); // Reset FIFO
				rLED = 1;//notifies the user that an over flow occured
			}

			// Print results
			for( idx = 0; idx < readECGSamples; idx++ ) {
				daplink.printf("%6d\r\n", ecgSample[idx]);

				bleGattAttrWrite(gattCharECG.getValueHandle(),
						(uint8_t *)&ecgSample[idx], sizeof(ecgSample[idx]));
			}
		}
	}
}

#endif

/******************************************************************************
************** MAX30003WING (BPM) *********************************************
*******************************************************************************/
#if defined(LIB_MAX30003)

#define MAX30003_IRQ_ASSERTED_SIGNAL_ID	1

MAX30003 max30003(spim2, SPI2_SS);		/* MAX30003WING board */

Thread thread_max30003_reader;

/* ECG FIFO nearly full callback */
void ecgFIFO_callback()
{
	thread_max30003_reader.signal_set(MAX30003_IRQ_ASSERTED_SIGNAL_ID);
}

void ecg_config(MAX30003& ecgAFE)
{
    // Reset ECG to clear registers
    ecgAFE.writeRegister( MAX30003::SW_RST , 0);

    // General config register setting
    MAX30003::GeneralConfiguration_u CNFG_GEN_r;
    CNFG_GEN_r.bits.en_ecg = 1;     // Enable ECG channel
    CNFG_GEN_r.bits.rbiasn = 1;     // Enable resistive bias on negative input
    CNFG_GEN_r.bits.rbiasp = 1;     // Enable resistive bias on positive input
    CNFG_GEN_r.bits.en_rbias = 1;   // Enable resistive bias
    CNFG_GEN_r.bits.imag = 2;       // Current magnitude = 10nA
    CNFG_GEN_r.bits.en_dcloff = 1;  // Enable DC lead-off detection
    ecgAFE.writeRegister( MAX30003::CNFG_GEN , CNFG_GEN_r.all);


    // ECG Config register setting
    MAX30003::ECGConfiguration_u CNFG_ECG_r;
    CNFG_ECG_r.bits.dlpf = 1;       // Digital LPF cutoff = 40Hz
    CNFG_ECG_r.bits.dhpf = 1;       // Digital HPF cutoff = 0.5Hz
    CNFG_ECG_r.bits.gain = 3;       // ECG gain = 160V/V
    CNFG_ECG_r.bits.rate = 2;       // Sample rate = 128 sps
    ecgAFE.writeRegister( MAX30003::CNFG_ECG , CNFG_ECG_r.all);


    //R-to-R configuration
    MAX30003::RtoR1Configuration_u CNFG_RTOR_r;
    CNFG_RTOR_r.bits.wndw = 0b0011;         // WNDW = 96ms
    CNFG_RTOR_r.bits.rgain = 0b1111;        // Auto-scale gain
    CNFG_RTOR_r.bits.pavg = 0b11;           // 16-average
    CNFG_RTOR_r.bits.ptsf = 0b0011;         // PTSF = 4/16
    CNFG_RTOR_r.bits.en_rtor = 1;           // Enable R-to-R detection
    ecgAFE.writeRegister( MAX30003::CNFG_RTOR1 , CNFG_RTOR_r.all);


    //Manage interrupts register setting
    MAX30003::ManageInterrupts_u MNG_INT_r;
    MNG_INT_r.bits.efit = 0b00011;          // Assert EINT w/ 4 unread samples
    MNG_INT_r.bits.clr_rrint = 0b01;        // Clear R-to-R on RTOR reg. read back
    ecgAFE.writeRegister( MAX30003::MNGR_INT , MNG_INT_r.all);


    //Enable interrupts register setting
    MAX30003::EnableInterrupts_u EN_INT_r;
    EN_INT_r.bits.en_eint = 1;              // Enable EINT interrupt
    EN_INT_r.bits.en_rrint = 1;             // Enable R-to-R interrupt
    EN_INT_r.bits.intb_type = 3;            // Open-drain NMOS with internal pullup
    ecgAFE.writeRegister( MAX30003::EN_INT , EN_INT_r.all);


    //Dyanmic modes config
    MAX30003::ManageDynamicModes_u MNG_DYN_r;
    MNG_DYN_r.bits.fast = 0;                // Fast recovery mode disabled
    ecgAFE.writeRegister( MAX30003::MNGR_DYN , MNG_DYN_r.all);

    // MUX Config
    MAX30003::MuxConfiguration_u CNFG_MUX_r;
    CNFG_MUX_r.bits.openn = 0;          // Connect ECGN to AFE channel
    CNFG_MUX_r.bits.openp = 0;          // Connect ECGP to AFE channel
    ecgAFE.writeRegister( MAX30003::CNFG_EMUX , CNFG_MUX_r.all);

    return;
}

void max30003_reader_task()
{
    // Constants
    const int EINT_STATUS =  1 << 23;
    const int RTOR_STATUS =  1 << 10;
    const int RTOR_REG_OFFSET = 10;
    const float RTOR_LSB_RES = 0.0078125f;
    const int FIFO_OVF =  0x7;
    const int FIFO_VALID_SAMPLE =  0x0;
    const int FIFO_FAST_SAMPLE =  0x1;
    const int ETAG_BITS = 0x7;

    InterruptIn ecgFIFO_int(P5_4);          // Config P5_4 as int. in for the
    ecgFIFO_int.fall(&ecgFIFO_callback);    // ecg FIFO almost full interrupt

    ecg_config(max30003);                   // Config ECG

    max30003.writeRegister( MAX30003::SYNCH , 0);

    uint32_t ecgFIFO, RtoR, readECGSamples, idx, ETAG[32], status;
    int16_t ecgSample[32];
    float BPM;
    Timer bleNotifyTimer;

    bleNotifyTimer.start();
    while (1) {
		// Read back ECG samples from the FIFO
		thread_max30003_reader.signal_wait(MAX30003_IRQ_ASSERTED_SIGNAL_ID);

		while (1) {
			/* Read back ECG samples from the FIFO */
			status = max30003.readRegister( MAX30003::STATUS );      // Read the STATUS register

			if (status & (RTOR_STATUS | EINT_STATUS) == 0) {
				break;
			}

			// Check if R-to-R interrupt asserted
			if ((status & RTOR_STATUS) == RTOR_STATUS) {

				daplink.printf("R-to-R Interrupt \r\n");

				// Read RtoR register
				RtoR = max30003.readRegister( MAX30003::RTOR ) >>  RTOR_REG_OFFSET;

				// Convert to BPM
				BPM = 1.0f / ( RtoR * RTOR_LSB_RES / 60.0f );

				daplink.printf("BPM: %.2f\r\n", BPM);

				if (bleNotifyTimer.read_ms() >=  (MAX30003_BLE_NOTIFY_PERIOD_SEC * 1000)) {
					bleGattAttrWrite(gattCharBPM.getValueHandle(), (uint8_t *)&BPM, sizeof(BPM));
					bleNotifyTimer.reset();
				}
			}

			// Check if EINT interrupt asserted
			if ((status & EINT_STATUS) == EINT_STATUS) {
				readECGSamples = 0;	// Reset sample counter

				do {
					ecgFIFO = max30003.readRegister( MAX30003::ECG_FIFO );	// Read FIFO
					ecgSample[readECGSamples] = ecgFIFO >> 8;				// Isolate voltage data
					ETAG[readECGSamples] = ( ecgFIFO >> 3 ) & ETAG_BITS;	// Isolate ETAG
					readECGSamples++;										// Increment sample counter

				// Check that sample is not last sample in FIFO
				} while (ETAG[readECGSamples-1] == FIFO_VALID_SAMPLE ||
						  ETAG[readECGSamples-1] == FIFO_FAST_SAMPLE);

				// Check if FIFO has overflowed
				if (ETAG[readECGSamples - 1] == FIFO_OVF){
					max30003.writeRegister( MAX30003::FIFO_RST , 0); // Reset FIFO
					rLED = 1;
				}
			}
		}

    }
}

#endif

/******************************************************************************
************** MAX11301WING ***************************************************
*******************************************************************************/
#if defined(LIB_MAX113XX_PIXI)

#define MAX113XX_DATA_READ_PERIOD_MSEC	2000
#define MAX113XX_I2C_ADDRESS	0x38

Thread thread_max11301_reader;

void max11301_reader_task()
{
    uint16_t adcData;
    float adcVoltage;

    MAX113XX_I2C pixi(i2c1, MAX113XX_I2C::MAX11301, MAX113XX_I2C_ADDRESS, P5_5);

    pixi.dacWrite(MAX113XX_Pixi::PORT0, 0x000);    // Pixi PORT0 is -5V
    pixi.dacWrite(MAX113XX_Pixi::PORT1, 0xFFF);    // Pixi PORT1 is +5V

    daplink.printf("Starting MAX11301 PIXI ADC Demo Application...\r\n");

    while (1) {
        pixi.singleEndedADCRead(MAX113XX_Pixi::PORT9, adcData);    // Read value from PORT9
        adcVoltage = -5 + 2.442e-3  * adcData;                     // Convert ADC val. to a voltage

        daplink.printf("ADC Read is : %i,\tVoltage is %1.3f V \r\n", adcData, adcVoltage);

        bleGattAttrWrite(gattCharADC.getValueHandle(), (uint8_t *)&adcVoltage, sizeof(adcVoltage));

        Thread::wait(MAX113XX_PIXI_BLE_NOTIFY_PERIOD_SEC * 1000);
    }
}
#endif

/******************************************************************************
 ******************************************************************************
 ******************************************************************************/

int main()
{
	osStatus status;
	rLED = LED_OFF; gLED = LED_OFF; bLED = LED_OFF; // red

	aliveLedEventId = eventQueue.call_every(1000, blinkCallback);

    daplink.printf("Initializing BLE service...\r\n");

	BLE &ble = BLE::Instance();
	ble.onEventsToProcess(scheduleBleEventsProcessing);
	ble.init(bleInitComplete);

#if defined(LIB_MAX30205)
	status = thread_max30205_reader.start(max30205_reader_task);
	if (status != osOK) {
		daplink.printf("Starting thread_max30205_reader thread failed(%d)!\r\n", status);
	}
#endif

#if defined(LIB_MAX30101)
	status = thread_max30101_reader.start(max30101_reader_task);
	if (status != osOK) {
		daplink.printf("Starting thread_max30205_reader thread failed(%d)!\r\n", status);
	}
#endif

#if defined(LIB_MAX30003)
	status = thread_max30003_reader.start(max30003_reader_task);
	if (status != osOK) {
		daplink.printf("Starting thread_max30205_reader thread failed(%d)!\r\n", status);
	}
#endif

#if defined(LIB_MAX113XX_PIXI)
	status = thread_max11301_reader.start(max11301_reader_task);
	if (status != osOK) {
		daplink.printf("Starting thread_max30205_reader thread failed(%d)!\r\n", status);
	}
#endif

	eventQueue.dispatch_forever();

	return 0;
}