File content as of revision 25:8621ebb6ea0c:

/** 
  * This is the project for "BLE HealthCare". The device is attached on any patient's body at will.
  
  * Revision:
  * version 0.8     02-12-2018
  * version 0.8.5   02-14-2018
  * version 0.9     02-15-2018  Pulse sensor and thermometer added 
  * version 0.9.5   02-16-2018  Calculation for pulse sensor and thermometer. GSM library added 
  * version 0.9.6   02-21-2018  Update mbed-os
  * version 0.9.6   02-21-2018  Some modification for LM35
  * version 0.9.8   03-04-2018  Data receiving from client device added
  * version 1.0     03-09-2018  Some minor bugs fixed
  * version 1.0.5   03-09-2018  Some minor bugs fixed
  * version 1.3.7   04-06-2018  Some minor bugs fixed
  * version 1.4.4   23-06-2018  Added max30100 still developing
 
/* ======================== INCLUDES ========================= */
#include <events/mbed_events.h>
#include <mbed.h>
#include "ble/BLE.h"
#include "ble_healthcare_service.h"
#include "LM35.h"
#include "PulseSensor.h"
#include "MAX30100_PulseOximeter.h"
#include "KalmanFilterPulse.h"
 
/* ======================== DEFINES ========================== */
#define PULSE_SENSOR_PIN        A2
#define THERM_SENSOR_PIN        A3
 
#define START_SEND_INT_TEMP     13
#define START_SEND_FLOAT_TEMP   10
#define STOP_SEND_TEMP          20

#define SAMPLE_HEARTRATE        16
#define AVERAGE_HEARTRATE       8
#define REPORTING_PERIOS        3125  /* 1/16 s */

/* ======================= VARIABLES ========================= */
/* GLOBAL VARIABLES */
static float    processedTemperature  = 36.9;
static uint16_t processedHRMCounter;
static float    sendCombinedTempAndHR;
static uint16_t sendCombinedHRAndTemp;

bool                 isEmptyavgHR                      = true;
bool                 startCalculatingStandardDeviation = true;
std::vector<float>   valuesHeartRate;
std::vector<uint8_t> valuesSpO2;
 
uint8_t avgHRIndex      = 0;    
float avgHR[AVERAGE_HEARTRATE] = {0};
uint16_t avgHeartRate;
 
/* PRIVATE VARIABLES */
uint8_t cnt;
 
uint8_t startSendFloat = 0;
 
bool    isConnectedToDevice = false;
 
/* No need to display the device name */
//const static char     DEVICE_NAME[] = "BODY SENSOR";
 
static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE, 
                                       GattService::UUID_HEALTH_THERMOMETER_SERVICE, 
                                       HealthCareService::USER_DATA_SERVICE_UUID,
                                       HealthCareService::DEVICE_INFO_SERVICE_UUID
                                       };
                                       
uint8_t htsPosition = HealthCareService::HealthCareService::TEMPERATURE_LOCATION_FINGER;
uint8_t hrmPosition = HealthCareService::HealthCareService::HRM_LOCATION_FINGER;                                       
 
/* STRUCTS/CLASSESS */
typedef struct {
    float    heartRate;
    float    SpO2;
} message_t;

HealthCareService       *HealthCareServicePtr;

MemoryPool<message_t, 32> mpool;
Queue<message_t, 32> queue;

Thread              thread;
PulseOximeter       pox; 
 
static EventQueue   eventQueue(EVENTS_EVENT_SIZE * 32);
Serial              serial(USBTX, USBRX);
KalmanFilterPulse   kalman(0.5, 10, 1);
KalmanFilterPulse   kalman1(2, 10, 0.1);
 
/* ================== FUNCTION PROTOTYPES ==================== */
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *event);
void onBleInitError(BLE &ble, ble_error_t error);
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params);
void onDataWrittenCallback(const GattWriteCallbackParams *params);
void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context);
 
void sendDataToProcessing(char symbol, int data);
void onBeatDetected();
void beatEvent();
void updatePayload(void);
void main_event(void);
void periodicCallback(void);
 
PulseSensor sensor(PULSE_SENSOR_PIN, sendDataToProcessing, 20);
LM35Therm   lm35(THERM_SENSOR_PIN, 4.56);

/* ==================== FUNCTION DETAILS ===================== */
/* Restart Advertising on disconnection */
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *event) 
{
    BLE::Instance().gap().startAdvertising();
    printf("Device disconnected with mobile/table\r\n");
    isConnectedToDevice = false;
}
 
void sendDataToProcessing(char symbol, int data)
{
    int filtedData;
    if (symbol == 'B')
    {
        filtedData = kalman.kalmanUpdate(data);
        filtedData = kalman.kalmanUpdate(filtedData);
        processedHRMCounter = filtedData/1;
        printf("%c%d %d\r\n", symbol, data,(uint8_t)(filtedData/1));
    }  
}
 
void updatePayload(void) 
{
    // Update the count in the SERVICE_DATA field of the advertising payload
    cnt++;
    uint8_t service_data[8];
    /* first 2 bytes are for service UUID */
    service_data[0] = HealthCareService::USER_DATA_SERVICE_UUID & 0xFF;
    service_data[1] = HealthCareService::USER_DATA_SERVICE_UUID >> 8;
    /* next 4 bytes are for client ID */
    service_data[2] = 0x07;
    service_data[3] = 0x09;
    service_data[4] = 0x9A;
    service_data[5] = 0xAC;
    /* last 2 bytes are sensor data */
    service_data[6] = cnt;
    service_data[7] = cnt;
    ble_error_t err = BLE::Instance().gap().updateAdvertisingPayload(GapAdvertisingData::SERVICE_DATA, (uint8_t *)service_data, sizeof(service_data));
 
}
 
void main_event(void) 
{
    uint16_t intTemperatureValuex100;
    uint8_t  fractionalTemperature;
    uint8_t  decimalTemperature;
    
    if (BLE::Instance().gap().getState().connected) 
    {
        isConnectedToDevice = true;
    }
    
    /* Do blocking calls or whatever is necessary for sensor polling.
       In our case, we simply update the Temperature measurement. */
    /* TODO Read temperature */
    lm35.getAverageValue();
    processedTemperature = kalman1.kalmanUpdate(lm35.getTempInC());
    processedTemperature = kalman1.kalmanUpdate(processedTemperature);    
    
    /* TODO Read Heart Rate */
    /* Updated in callback function */
    
    /* Some little tricks here to make the temperature decimal and fractional parts different from the send codes */
    intTemperatureValuex100 = processedTemperature * 100;
    fractionalTemperature   = intTemperatureValuex100 % 100;
    decimalTemperature      = intTemperatureValuex100 / 100;
    if ((fractionalTemperature == START_SEND_INT_TEMP)   || 
        (fractionalTemperature == START_SEND_FLOAT_TEMP) ||
        (fractionalTemperature == STOP_SEND_TEMP)        ||
        (fractionalTemperature == decimalTemperature)) 
    {
        fractionalTemperature = fractionalTemperature + 1;
    }
    
    
    /* TODO Update Service data */
    updatePayload();
    
    /* sendCombinedTempAndHR = (processedTemperature * 100)* 1000 + processedHRMCounter */
    sendCombinedTempAndHR  = intTemperatureValuex100 * 10.0; /* Temperature float to int conversion */
    sendCombinedTempAndHR  = sendCombinedTempAndHR + (float)(processedHRMCounter/100.0); 
    
    switch (startSendFloat) {
        case 0: sendCombinedHRAndTemp  = processedHRMCounter * 100;
                sendCombinedHRAndTemp  = sendCombinedHRAndTemp + (uint8_t)START_SEND_INT_TEMP; 
        break;
        
        case 1: /* sendCombinedHRAndTemp = (processedHRMCounter * 100) + decimalTemperature */
                /* Because the maximum size of HRM Value is 2 bytes */
                sendCombinedHRAndTemp  = processedHRMCounter * 100;
                sendCombinedHRAndTemp  = sendCombinedHRAndTemp + (uint8_t)decimalTemperature; 
        break;
                
        case 2: sendCombinedHRAndTemp  = processedHRMCounter * 100;
                sendCombinedHRAndTemp  = sendCombinedHRAndTemp + (uint8_t)START_SEND_FLOAT_TEMP; 
        break;
        
        case 3: sendCombinedHRAndTemp  = processedHRMCounter * 100;
                sendCombinedHRAndTemp  = sendCombinedHRAndTemp + (uint8_t)fractionalTemperature; 
        break;
                
        default: break;
    }   
//    printf("sendCombinedTempAndHR %d\r\n", sendCombinedTempAndHR);
//    printf("sendCombinedTempAndHR %.2f\r\n\r\n", (float)sendCombinedTempAndHR);
//    printf("processedHRMCounter %d\r\n", processedHRMCounter);
//    printf("processedTemperature %d\r\n", (uint8_t)processedTemperature);
//    printf("sendCombinedHRAndTemp %d\r\n", sendCombinedHRAndTemp);
    
    if (isConnectedToDevice) 
    {
        HealthCareServicePtr->updateTemperature(sendCombinedTempAndHR);
        HealthCareServicePtr->updateHeartRate(sendCombinedHRAndTemp);    
        startSendFloat = (startSendFloat + 1) % 4;   
    }
    else 
    {
        startSendFloat = 0;
    }
}

void beatEvent()
{
    float       heartRate;
    float       rawHeartRate;
    uint8_t     spO2;
    uint16_t    rawSpO2;
    uint8_t     counterMAX30100  = 0;
    bool        newValueMAX30100 = false;
    uint32_t    loopCount = 0;
    printf("Thread\r\n");
    while (1)
    {
        pox.update();
        if (loopCount >= REPORTING_PERIOS) 
        {
            heartRate = pox.getHeartRate();
            spO2 = pox.getSpO2();
            
            if(heartRate != 0 && spO2 != 0) 
            {
                printf("Heart rate: %f",heartRate);
                printf("   bpm / SpO2: %d%\r\n", spO2);
                valuesHeartRate.push_back(heartRate);
                valuesSpO2.push_back(spO2);
                counterMAX30100++;
            }
            else
            {
                printf("No finger\r\n");
            }
            
            if(counterMAX30100 == SAMPLE_HEARTRATE) {
                rawHeartRate = 0;
                rawSpO2 = 0;
                for(int i = 0; i < SAMPLE_HEARTRATE; i++){
                    rawHeartRate += valuesHeartRate[i];
                    rawSpO2 += valuesSpO2[i];        
                }
                
                rawHeartRate /= SAMPLE_HEARTRATE;
                rawSpO2 /= SAMPLE_HEARTRATE;
                
                counterMAX30100 = 0;
                valuesHeartRate.clear();
                valuesSpO2.clear();
                newValueMAX30100 = true;
            }
            loopCount = 0;
        }
        if (newValueMAX30100)
        {
             message_t *message = mpool.alloc();
             message->heartRate = rawHeartRate;
             message->SpO2      = rawSpO2;
             printf("Send vales %.2f %.2f \r\n", rawHeartRate, rawSpO2);
             queue.put(message);                     
             newValueMAX30100 = false;
        } 
        loopCount++;                    
    }
}
 
void periodicCallback(void) 
{
    /* call main_event immediately */
    eventQueue.call(main_event);
}
 
void printMacAddress() {
    /* Print out device MAC address to the console*/
    Gap::AddressType_t addr_type;
    Gap::Address_t address;
    BLE::Instance().gap().getAddress(&addr_type, address);
    printf("DEVICE MAC ADDRESS: ");
    for (int i = 5; i >= 1; i--) {
        printf("%02x:", address[i]);
    }
    printf("%02x\r\n", address[0]);
}
 
void onBleInitError(BLE &ble, ble_error_t error) 
{
   /* Initialization error handling should go here */
   printf("BLE init error!\r\n");
}
 
/**
 * This callback allows the HealthCareService to receive updates to the controlState Characteristic.
 *
 * @param[in] params
 *     Information about the characterisitc being updated.
 */
void onDataWrittenCallback(const GattWriteCallbackParams *params) 
{
//    printf("Write callback, value %d\r\n", *(params->data));
    if ((params->handle == HealthCareServicePtr->getTypeHandle()) && (params->len >= 1)) 
    {
        uint8_t oldPosition = htsPosition;
        htsPosition = *(params->data);
        printf("Old type: %d, New type %d\r\n", oldPosition, htsPosition);
        HealthCareServicePtr->updateType(htsPosition);
    }
    if ((params->handle == HealthCareServicePtr->getLocationHandle()) && (params->len >= 1)) 
    {
        uint8_t oldPosition = hrmPosition;
        hrmPosition = *(params->data);
        printf("Old location: %d, New location %d\r\n", oldPosition, hrmPosition);
        HealthCareServicePtr->updateLocation(hrmPosition);
    }    
}
 
/** 
  * @brief Callback triggered when the ble initialization process has finished
  */
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params) 
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;
    uint8_t service_data[8];
 
    if (error != BLE_ERROR_NONE) 
    {
        onBleInitError(ble, error);
        return;
    }
 
    /* Ensure that it is the default instance of BLE */
    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) 
    {
        return;
    }
    uint8_t initial_HRMIncreasement = 1;
    ble.gap().onDisconnection(&disconnectionCallback);
    ble.gattServer().onDataWritten(onDataWrittenCallback);
    
    HealthCareServicePtr = new HealthCareService(ble, processedTemperature, htsPosition, 
                                                      processedHRMCounter, hrmPosition,
                                                      initial_HRMIncreasement);
 
    /* 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::GENERIC_THERMOMETER );
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
    
    /* No need to display the device name */
    // ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME , (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    /* uint8_t service_data[8]; */
    /* first 2 bytes are for service UUID */
    service_data[0] = HealthCareService::USER_DATA_SERVICE_UUID & 0xFF;
    service_data[1] = HealthCareService::USER_DATA_SERVICE_UUID >> 8;
    /* next 4 bytes are for client ID */
    service_data[2] = 0x07;
    service_data[3] = 0x09;
    service_data[4] = 0x89;
    service_data[5] = 0xAB;
    /* last 2 bytes are sensor data */
    service_data[6] = cnt;
    service_data[7] = cnt;
 
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SERVICE_DATA , (uint8_t *)service_data, sizeof(service_data));
    
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
    ble.gap().startAdvertising();
    
    printMacAddress();
    printf("BLE init successfully\r\n");
}
 
void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) 
{
    BLE &ble = BLE::Instance();
    eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
}
 
/* MAIN FUNCTION */
int main() 
{
    serial.baud(115200);
    printf("\r\n BODY WIRELESS SENSOR NETWORK\r\n");
    
//    if(!pox.begin())
//    {
//        printf("No sensor detected\r\n");
//    }
//    else
//    {
//        printf("Sensor found\r\n");
//    }
//    thread.start(callback(beatEvent));    
    
    /* call periodicCallback every 500ms */
    eventQueue.call_every(1000, periodicCallback);
    
    /* init BLE */
    BLE &ble = BLE::Instance();
    ble.onEventsToProcess(scheduleBleEventsProcessing);
    ble.init(bleInitComplete);
    
    /* dispatch the event queue */
    sensor.start();
    eventQueue.dispatch_forever();
 
    return 0;
}