File content as of revision 29:6ba8491c1dab:

#include "mbed.h"
#include "ble/BLE.h"
#include "VL53L0X.h"
#include "ble/services/HeartRateService.h"

#define range1_addr         (0x56)
#define range2_addr         (0x60)
#define range1_XSHUT        p15
#define range2_XSHUT        p16
#define VL53L0_I2C_SDA      p30 
#define VL53L0_I2C_SCL      p7  
#define TIME_SCALE          2 // sensors activated every 100ms * TIME_SCALE
#define DIST_MIN            0
#define DIST_MAX            22

const static int cw = 20 / TIME_SCALE;
int countdown = cw;
bool countdown1_triggered = false, countdown2_triggered = false;
bool step_in = false, step_out = false;
 
Serial pc(USBTX, USBRX);
static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); 
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led(LED3, 1);
uint16_t customServiceUUID  = 0xA000;
uint16_t readCharUUID       = 0xA001;
uint16_t writeCharUUID      = 0xA002;

static DigitalOut shutdown1_pin(range1_XSHUT);
static VL53L0X range1(&devI2c, &shutdown1_pin, NC);
static DigitalOut shutdown2_pin(range2_XSHUT);
static VL53L0X range2(&devI2c, &shutdown2_pin, NC);

uint32_t distance1, distance2;
int dist1, dist2, status1, status2;

const static char     DEVICE_NAME[]        = "OCCUPY-CRICHTON-ST";
static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE};

HeartRateService         *hrService;
uint8_t hrmCounter = 0; 

void connectionCallback(const Gap::ConnectionCallbackParams_t *) {
    printf("Bluetooth connected\n");
}

void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *)
{
    printf("Bluetooth disconnected\n");
    BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
}

void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE &ble          = params->ble;
    ble_error_t error = params->error;
    
    if (error != BLE_ERROR_NONE) {
        return;
    }
    
    ble.gap().onConnection(connectionCallback);
    ble.gap().onDisconnection(disconnectionCallback);
    hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);

    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); 
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);

    ble.gap().startAdvertising();
}

int format_dist(int distance) {
   int result;
 
   if (distance > 1270)
      result = 127;
   else
      result = distance/10;
 
   return result; 
}

void wakeup_event_cb() {
    led != led;
    
    if (countdown1_triggered) {
        countdown--;
        if (countdown == 0) {
            countdown1_triggered = false;
        }
    }
    if (countdown2_triggered) {

        countdown--;
        if (countdown == 0) {
            countdown2_triggered = false;
        }
    }
    
    status1 = range1.get_distance(&distance1);
    status2 = range2.get_distance(&distance2);
    
    dist1 = format_dist(distance1);
    dist2 = format_dist(distance2);

    if (status1 == VL53L0X_ERROR_NONE) {
//        printf("Range1 [mm]:            %6ld\r\n", dist1);

        if (dist1 > DIST_MIN && dist1 < DIST_MAX) {
            led1 = 0;
            
            if (!countdown1_triggered && !countdown2_triggered) {
                countdown1_triggered = true;
                countdown = cw;
            } else if (countdown2_triggered) {
                printf("STEP IN DETECTED\n");
                countdown2_triggered = false;
            }
            
            hrmCounter = dist1;
            hrService->updateHeartRate(hrmCounter);
        }
        else {
            led1 = 1;
        }
    } else {
//        printf("Range1 [mm]:                --\r\n");
        led1 = 1;
    }
    if (status2 == VL53L0X_ERROR_NONE) {
        if (dist2 > DIST_MIN && dist2 < DIST_MAX) {
            led2 = 0;
            
            if (!countdown1_triggered && !countdown2_triggered) {
                countdown2_triggered = true;
                countdown = cw;
            } else if (countdown1_triggered) {
                printf("STEP OUT DETECTED\n");
                countdown1_triggered = false;
            }
            
            hrmCounter = dist2;
            hrService->updateHeartRate(hrmCounter);
        }
    } else {
//        printf("Range2 [mm]:                --\r\n");
        led2 = 1;
    }
}

int main(void)
{
    range1.init_sensor(range1_addr);
    range2.init_sensor(range2_addr);
    
    printf("\n\r********* Starting Main Loop *********\n\r");
  
    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
    ble.init(bleInitComplete);
    
    Ticker ticker;
    ticker.attach(wakeup_event_cb, 0.1*TIME_SCALE);
    
    while (ble.hasInitialized()) {
        ble.waitForEvent(); 
    }
}