File content as of revision 48:78310b56de00:

#include <stdio.h>
#include <math.h>
#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        p16
#define range2_XSHUT        p15
#define VL53L0_I2C_SDA      p30 
#define VL53L0_I2C_SCL      p7  
#define TIME_SCALE          3 // sensors activated every 100ms * TIME_SCALE = 0.3 seconds
#define DIST_MIN            20 // PROD
#define DIST_MAX            120 // PROD
//#define DIST_MIN            0 // DEV
//#define DIST_MAX            22 // DEV
//#define TIMESTAMP_FREQ      3000 // PROD: time granularity is 100ms * TIMESTAMP_FREQ = 5 minutes
#define TIMESTAMP_FREQ      300 // DEV: time granularity is 100ms * TIMESTAMP_FREQ = 30 seconds
//#define MAX_TIME            1728000 // PROD: keep track of 100ms * MAX_TIME = 2880 minutes = 2 days
#define MAX_TIME            172800 // DEV: keep track of 100ms * MAX_TIME = 288 minutes = 4.8 hours
 
Serial pc(USBTX, USBRX);
static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); 
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led(LED3, 1);
Ticker ticker;
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;
int packet_queue[4000] = {0};
int packet_queue_index = 2;
int current_index = 0, send_count = 0, triple_send_count = 0;
 
const static int cw = 20 / TIME_SCALE;
const static int time_cycle = TIMESTAMP_FREQ / TIME_SCALE;
const static int maximum_time = MAX_TIME / TIMESTAMP_FREQ;
int countdown = cw;
bool countdown1_triggered = false, countdown2_triggered = false;
bool step_in = false, step_out = false;
bool range1_triggered = false, range2_triggered = false;
bool master_connected = false, connection_triggered = false;
bool first_loop_run = true, enter_send_loop = true;
 
const static char     DEVICE_NAME[]        = "OCCUPY-CRICHTON-ST"; 
static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE};
int * encoded_array;
int timestamp = time_cycle, current_time = 0;
 
HeartRateService         *hrService;
uint8_t hrmCounter = 0; 
 
// converts distance to cm
int format_dist(int distance) {
   int result;
   if (distance > 2550)
      result = 255;
   else
      result = distance/10;
   return result; 
}
 
// starts the clock after one sensor was triggered: the next must be triggered 
// within 2 seconds for a step to be counted
void check_for_countdown(bool countdown_triggered) {
    if (countdown_triggered) {
        ::countdown--;
        if (countdown == 0) {
            countdown_triggered = false;
        }
    }
}

// needed for the stalling of the queue after the first 4 packets are transmitted:
// pads queue with "fake" packets that will not be transmitted
void optimize_transmissions(int packet_queue[]) {
    for (int count = 4; count < 12; count++) {
        packet_queue[count] = 1;
    }
}
 
void dec_to_bin(int decimal, int bin[]) {
    int temp;
    // encoding little-endian
    for (int i = 10; i >= 0; i--) {
        temp = decimal >> i;
        bin[i] = temp&1;
    }
}
 
int bin_to_dec(int binary) {
    int rem, temp, dec = 0, b = 1;
    temp = binary;
    while (temp > 0)
    {
        rem = temp % 10;
        dec = dec + rem * b;
        b *= 2;
        temp /= 10;
    }
    return dec;
}

// creates the encoding that fits all required info into 2 bytes
void encode_bin(int direction, int time, int encoding[]) {
    time = time % maximum_time;
    int bin_timestamp[11] = {0};
    dec_to_bin(time, bin_timestamp); 
    
    // for checking transmission errors:
    // sending as 2 messages; designating them with 0 (1st part) and 1 (2nd part)
    encoding [8] = 1;
    encoding [0] = 0;
 
    // used to send the current time when Bluetooth connection established
    if (direction == -1) {
        encoding[15] = 1;
        encoding[7] = 1;
        
    } else {
        encoding[15] = 0;
        encoding[7] = 0;
        
        // to distinguish whether movement was IN (1) or OUT (0)
        if (direction == 1) {
            encoding[1] = 1;
        } else {
            encoding[1] = 0;
        }
    }
    
    int count = 10;
    for (int i = 14; i >= 2; i--) {
        if (i == 8 || i == 7) { }
        else {
            encoding[i] = bin_timestamp[count];
            count--;
        }
     }
}
 
// converts the binary encoding back into 2 byte-sized packets
void create_packets(int encoding[], double& packet1, double& packet2) {
    double binary1 = 0, binary2 = 0;
    for (int i = 0; i < 8; i++) {
        binary1 += encoding[i] * pow(10, (double)i);
        binary2 += encoding[8+i] * pow(10, (double)i);
    }
    packet1 = bin_to_dec(binary1);
    packet2 = bin_to_dec(binary2);
}
 
void send_packet() {     
    hrmCounter = packet_queue[current_index];
    hrService->updateHeartRate(hrmCounter);
    printf("sent packet %i\r\n", hrmCounter);
    ::current_index++;
}
 
// steps are added to a queue to be sent as soon as the gateway connects
void enqueue_packet(int direction, int time, int packet_queue[], int priority) { 
    int encoding [16] = {0};
    encode_bin(direction, time, encoding);
    double packet1, packet2;
    create_packets(encoding, packet1, packet2);
    int p1 = (int)packet1;
    int p2 = (int)packet2;
   
    if (priority == 1) {
        packet_queue[0] = p1;
        packet_queue[1] = p2;
    
        printf("sudo enqueued packet %i\r\n",  packet_queue[0]);
        printf("sudo enqueued packet %i\r\n",  packet_queue[1]);
    } else {        
        if (packet_queue_index == 4) {
            ::packet_queue_index = 12;
        } 
        packet_queue[packet_queue_index] = p1;
        packet_queue[packet_queue_index + 1] = p2;
    
        printf("enqueued packet %i\r\n",  packet_queue[packet_queue_index]);
        printf("enqueued packet %i\r\n",  packet_queue[packet_queue_index+1]);
    
        ::packet_queue_index += 2;
    }
}
 
// the end of the queue transmission is signalled so gateway users know
// when it is safe to disconnect
void enqueue_transmission_break() {
    int p1 = 0;
    int p2 = 0;
    
    packet_queue[packet_queue_index] = p1;
    packet_queue[packet_queue_index + 1] = p2;
    printf("enqueued transmission breaks %i and %i\r\n", packet_queue[packet_queue_index], packet_queue[packet_queue_index+1]);
    
    ::packet_queue_index += 2;
}
 
// adapted from code found at https://os.mbed.com/users/jkjk010695/code/Multi_VL53L0X/
// checks sensor readings to determine whether a step has been taken
void check_for_step(int direction, int status, int dist, DigitalOut led, bool& this_countdown_triggered, 
                    bool& other_countdown_triggered, bool& this_range_triggered, bool& other_range_triggered,
                    int packet_queue[]) {
    if (status == VL53L0X_ERROR_NONE) {
        if (dist > DIST_MIN && dist < DIST_MAX) {
            ::connection_triggered = true;
            led = 0;
            printf("Range%i [mm]:            %6ld\r\n", direction+1, dist);
            if (!this_countdown_triggered && !other_countdown_triggered) {
                this_countdown_triggered = true;
                ::countdown = cw;
            } else if (other_countdown_triggered && !this_range_triggered) {
                printf("STEP %i DETECTED\r\n", direction);
                enqueue_packet(direction, current_time, packet_queue, 0);
                other_countdown_triggered = false;
            }
            this_range_triggered = true;
        } else {
            led = 1;
            this_range_triggered = false;
        }
    } else {
        led = 1;
        this_range_triggered = false;
    }
}
 
// when connected to gateway, the HEAD and TAIL are added to the queue
void connectionCallback(const Gap::ConnectionCallbackParams_t *) {
    printf("Bluetooth connected at %i\r\n", current_time);
    ::master_connected = true;
    ::connection_triggered = false;
    enqueue_packet(-1, current_time, packet_queue, 1);
    enqueue_transmission_break();
    BLE::Instance(BLE::DEFAULT_INSTANCE).gap().stopAdvertising();
    wait(5);
}
 
// when disconnected from gateway, all variables reset to the initial state
// as though system had restarted
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *)
{
    printf("Bluetooth disconnected at %i\r\n", current_time);
    ::master_connected = false;
    ::timestamp = time_cycle;
    ::current_index = 0;
    ::packet_queue_index = 2;
    ::send_count = 0;
    ::triple_send_count = 0;
    BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
}

// standard BLE init code found at https://os.mbed.com/teams/Bluetooth-Low-Energy/code/BLE_HeartRate/
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().setAdvertisingInterval(100);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
 
    ble.gap().startAdvertising();
}
 
// called every 300ms to check whether a step has been taken and whether there
// is anything to be transmitted
void wakeup_event_cb() {
    ::timestamp++;
    ::current_time = timestamp / time_cycle;
    
    if (send_count % 2 == 0) {
        if (enter_send_loop && connection_triggered && (current_index < packet_queue_index) && master_connected) {
            send_packet();
        }
        ::triple_send_count++;
    }
    ::send_count++;
    
    if ((triple_send_count / 4) % 3 == 0) {
        ::enter_send_loop = true;
    } else { 
        ::enter_send_loop = false;
    }
    
    check_for_countdown(countdown1_triggered);
    check_for_countdown(countdown2_triggered);
        
    status1 = range1.get_distance(&distance1);
    status2 = range2.get_distance(&distance2);
    
    dist1 = format_dist(distance1);
    dist2 = format_dist(distance2);
    
    check_for_step(1, status1, dist1, led1, countdown1_triggered, countdown2_triggered, 
                    range1_triggered, range2_triggered, packet_queue);
    check_for_step(0, status2, dist2, led2, countdown2_triggered, countdown1_triggered, 
                    range2_triggered, range1_triggered, packet_queue);
}
 
// system put to sleep by waitForEvent() call and periodically interrupted
// by Ticker to check for steps and packets to send
int main(void)
{
    range1.init_sensor(range1_addr);
    range2.init_sensor(range2_addr);
    
    optimize_transmissions(packet_queue);
    
    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
    ble.init(bleInitComplete);
    
    ticker.attach(wakeup_event_cb, 0.1 * TIME_SCALE);
    
    while (ble.hasInitialized()) {
        ble.waitForEvent(); 
    }
}