main.cpp

00001 #include <stdio.h>
00002 #include <math.h>
00003 #include "mbed.h"
00004 #include "ble/BLE.h"
00005 #include "VL53L0X.h"
00006 #include "ble/services/HeartRateService.h"
00007  
00008 #define range1_addr         (0x56)
00009 #define range2_addr         (0x60)
00010 #define range1_XSHUT        p16
00011 #define range2_XSHUT        p15
00012 #define VL53L0_I2C_SDA      p30 
00013 #define VL53L0_I2C_SCL      p7  
00014 #define TIME_SCALE          3 // sensors activated every 100ms * TIME_SCALE = 0.3 seconds
00015 #define DIST_MIN            20 // PROD
00016 #define DIST_MAX            120 // PROD
00017 //#define DIST_MIN            0 // DEV
00018 //#define DIST_MAX            22 // DEV
00019 //#define TIMESTAMP_FREQ      3000 // PROD: time granularity is 100ms * TIMESTAMP_FREQ = 5 minutes
00020 #define TIMESTAMP_FREQ      300 // DEV: time granularity is 100ms * TIMESTAMP_FREQ = 30 seconds
00021 //#define MAX_TIME            1728000 // PROD: keep track of 100ms * MAX_TIME = 2880 minutes = 2 days
00022 #define MAX_TIME            172800 // DEV: keep track of 100ms * MAX_TIME = 288 minutes = 4.8 hours
00023  
00024 Serial pc(USBTX, USBRX);
00025 static DevI2C devI2c(VL53L0_I2C_SDA, VL53L0_I2C_SCL); 
00026 DigitalOut led1(LED1);
00027 DigitalOut led2(LED2);
00028 DigitalOut led(LED3, 1);
00029 Ticker ticker;
00030 uint16_t customServiceUUID  = 0xA000;
00031 uint16_t readCharUUID       = 0xA001;
00032 uint16_t writeCharUUID      = 0xA002;
00033  
00034 static DigitalOut shutdown1_pin(range1_XSHUT);
00035 static VL53L0X range1(&devI2c, &shutdown1_pin, NC);
00036 static DigitalOut shutdown2_pin(range2_XSHUT);
00037 static VL53L0X range2(&devI2c, &shutdown2_pin, NC);
00038  
00039 uint32_t distance1, distance2;
00040 int dist1, dist2, status1, status2;
00041 int packet_queue[4000] = {0};
00042 int packet_queue_index = 2;
00043 int current_index = 0, send_count = 0, triple_send_count = 0;
00044  
00045 const static int cw = 20 / TIME_SCALE;
00046 const static int time_cycle = TIMESTAMP_FREQ / TIME_SCALE;
00047 const static int maximum_time = MAX_TIME / TIMESTAMP_FREQ;
00048 int countdown = cw;
00049 bool countdown1_triggered = false, countdown2_triggered = false;
00050 bool step_in = false, step_out = false;
00051 bool range1_triggered = false, range2_triggered = false;
00052 bool master_connected = false, connection_triggered = false;
00053 bool first_loop_run = true, enter_send_loop = true;
00054  
00055 const static char     DEVICE_NAME[]        = "OCCUPY-CRICHTON-ST"; 
00056 static const uint16_t uuid16_list[]        = {GattService::UUID_HEART_RATE_SERVICE};
00057 int * encoded_array;
00058 int timestamp = time_cycle, current_time = 0;
00059  
00060 HeartRateService         *hrService;
00061 uint8_t hrmCounter = 0; 
00062  
00063 // converts distance to cm
00064 int format_dist(int distance) {
00065    int result;
00066    if (distance > 2550)
00067       result = 255;
00068    else
00069       result = distance/10;
00070    return result; 
00071 }
00072  
00073 // starts the clock after one sensor was triggered: the next must be triggered 
00074 // within 2 seconds for a step to be counted
00075 void check_for_countdown(bool countdown_triggered) {
00076     if (countdown_triggered) {
00077         ::countdown--;
00078         if (countdown == 0) {
00079             countdown_triggered = false;
00080         }
00081     }
00082 }
00083 
00084 // needed for the stalling of the queue after the first 4 packets are transmitted:
00085 // pads queue with "fake" packets that will not be transmitted
00086 void optimize_transmissions(int packet_queue[]) {
00087     for (int count = 4; count < 12; count++) {
00088         packet_queue[count] = 1;
00089     }
00090 }
00091  
00092 void dec_to_bin(int decimal, int bin[]) {
00093     int temp;
00094     // encoding little-endian
00095     for (int i = 10; i >= 0; i--) {
00096         temp = decimal >> i;
00097         bin[i] = temp&1;
00098     }
00099 }
00100  
00101 int bin_to_dec(int binary) {
00102     int rem, temp, dec = 0, b = 1;
00103     temp = binary;
00104     while (temp > 0)
00105     {
00106         rem = temp % 10;
00107         dec = dec + rem * b;
00108         b *= 2;
00109         temp /= 10;
00110     }
00111     return dec;
00112 }
00113 
00114 // creates the encoding that fits all required info into 2 bytes
00115 void encode_bin(int direction, int time, int encoding[]) {
00116     time = time % maximum_time;
00117     int bin_timestamp[11] = {0};
00118     dec_to_bin(time, bin_timestamp); 
00119     
00120     // for checking transmission errors:
00121     // sending as 2 messages; designating them with 0 (1st part) and 1 (2nd part)
00122     encoding [8] = 1;
00123     encoding [0] = 0;
00124  
00125     // used to send the current time when Bluetooth connection established
00126     if (direction == -1) {
00127         encoding[15] = 1;
00128         encoding[7] = 1;
00129         
00130     } else {
00131         encoding[15] = 0;
00132         encoding[7] = 0;
00133         
00134         // to distinguish whether movement was IN (1) or OUT (0)
00135         if (direction == 1) {
00136             encoding[1] = 1;
00137         } else {
00138             encoding[1] = 0;
00139         }
00140     }
00141     
00142     int count = 10;
00143     for (int i = 14; i >= 2; i--) {
00144         if (i == 8 || i == 7) { }
00145         else {
00146             encoding[i] = bin_timestamp[count];
00147             count--;
00148         }
00149      }
00150 }
00151  
00152 // converts the binary encoding back into 2 byte-sized packets
00153 void create_packets(int encoding[], double& packet1, double& packet2) {
00154     double binary1 = 0, binary2 = 0;
00155     for (int i = 0; i < 8; i++) {
00156         binary1 += encoding[i] * pow(10, (double)i);
00157         binary2 += encoding[8+i] * pow(10, (double)i);
00158     }
00159     packet1 = bin_to_dec(binary1);
00160     packet2 = bin_to_dec(binary2);
00161 }
00162  
00163 void send_packet() {     
00164     hrmCounter = packet_queue[current_index];
00165     hrService->updateHeartRate(hrmCounter);
00166     printf("sent packet %i\r\n", hrmCounter);
00167     ::current_index++;
00168 }
00169  
00170 // steps are added to a queue to be sent as soon as the gateway connects
00171 void enqueue_packet(int direction, int time, int packet_queue[], int priority) { 
00172     int encoding [16] = {0};
00173     encode_bin(direction, time, encoding);
00174     double packet1, packet2;
00175     create_packets(encoding, packet1, packet2);
00176     int p1 = (int)packet1;
00177     int p2 = (int)packet2;
00178    
00179     if (priority == 1) {
00180         packet_queue[0] = p1;
00181         packet_queue[1] = p2;
00182     
00183         printf("sudo enqueued packet %i\r\n",  packet_queue[0]);
00184         printf("sudo enqueued packet %i\r\n",  packet_queue[1]);
00185     } else {        
00186         if (packet_queue_index == 4) {
00187             ::packet_queue_index = 12;
00188         } 
00189         packet_queue[packet_queue_index] = p1;
00190         packet_queue[packet_queue_index + 1] = p2;
00191     
00192         printf("enqueued packet %i\r\n",  packet_queue[packet_queue_index]);
00193         printf("enqueued packet %i\r\n",  packet_queue[packet_queue_index+1]);
00194     
00195         ::packet_queue_index += 2;
00196     }
00197 }
00198  
00199 // the end of the queue transmission is signalled so gateway users know
00200 // when it is safe to disconnect
00201 void enqueue_transmission_break() {
00202     int p1 = 0;
00203     int p2 = 0;
00204     
00205     packet_queue[packet_queue_index] = p1;
00206     packet_queue[packet_queue_index + 1] = p2;
00207     printf("enqueued transmission breaks %i and %i\r\n", packet_queue[packet_queue_index], packet_queue[packet_queue_index+1]);
00208     
00209     ::packet_queue_index += 2;
00210 }
00211  
00212 // adapted from code found at https://os.mbed.com/users/jkjk010695/code/Multi_VL53L0X/
00213 // checks sensor readings to determine whether a step has been taken
00214 void check_for_step(int direction, int status, int dist, DigitalOut led, bool& this_countdown_triggered, 
00215                     bool& other_countdown_triggered, bool& this_range_triggered, bool& other_range_triggered,
00216                     int packet_queue[]) {
00217     if (status == VL53L0X_ERROR_NONE) {
00218         if (dist > DIST_MIN && dist < DIST_MAX) {
00219             ::connection_triggered = true;
00220             led = 0;
00221             printf("Range%i [mm]:            %6ld\r\n", direction+1, dist);
00222             if (!this_countdown_triggered && !other_countdown_triggered) {
00223                 this_countdown_triggered = true;
00224                 ::countdown = cw;
00225             } else if (other_countdown_triggered && !this_range_triggered) {
00226                 printf("STEP %i DETECTED\r\n", direction);
00227                 enqueue_packet(direction, current_time, packet_queue, 0);
00228                 other_countdown_triggered = false;
00229             }
00230             this_range_triggered = true;
00231         } else {
00232             led = 1;
00233             this_range_triggered = false;
00234         }
00235     } else {
00236         led = 1;
00237         this_range_triggered = false;
00238     }
00239 }
00240  
00241 // when connected to gateway, the HEAD and TAIL are added to the queue
00242 void connectionCallback(const Gap::ConnectionCallbackParams_t *) {
00243     printf("Bluetooth connected at %i\r\n", current_time);
00244     ::master_connected = true;
00245     ::connection_triggered = false;
00246     enqueue_packet(-1, current_time, packet_queue, 1);
00247     enqueue_transmission_break();
00248     BLE::Instance(BLE::DEFAULT_INSTANCE).gap().stopAdvertising();
00249     wait(5);
00250 }
00251  
00252 // when disconnected from gateway, all variables reset to the initial state
00253 // as though system had restarted
00254 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *)
00255 {
00256     printf("Bluetooth disconnected at %i\r\n", current_time);
00257     ::master_connected = false;
00258     ::timestamp = time_cycle;
00259     ::current_index = 0;
00260     ::packet_queue_index = 2;
00261     ::send_count = 0;
00262     ::triple_send_count = 0;
00263     BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
00264 }
00265 
00266 // standard BLE init code found at https://os.mbed.com/teams/Bluetooth-Low-Energy/code/BLE_HeartRate/
00267 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
00268 {
00269     BLE &ble          = params->ble;
00270     ble_error_t error = params->error;
00271     
00272     if (error != BLE_ERROR_NONE) {
00273         return;
00274     }
00275     
00276     ble.gap().onConnection(connectionCallback);
00277     ble.gap().onDisconnection(disconnectionCallback);
00278     hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
00279  
00280     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
00281     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00282     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
00283     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
00284     ble.gap().setAdvertisingInterval(100);
00285     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
00286  
00287     ble.gap().startAdvertising();
00288 }
00289  
00290 // called every 300ms to check whether a step has been taken and whether there
00291 // is anything to be transmitted
00292 void wakeup_event_cb() {
00293     ::timestamp++;
00294     ::current_time = timestamp / time_cycle;
00295     
00296     if (send_count % 2 == 0) {
00297         if (enter_send_loop && connection_triggered && (current_index < packet_queue_index) && master_connected) {
00298             send_packet();
00299         }
00300         ::triple_send_count++;
00301     }
00302     ::send_count++;
00303     
00304     if ((triple_send_count / 4) % 3 == 0) {
00305         ::enter_send_loop = true;
00306     } else { 
00307         ::enter_send_loop = false;
00308     }
00309     
00310     check_for_countdown(countdown1_triggered);
00311     check_for_countdown(countdown2_triggered);
00312         
00313     status1 = range1.get_distance(&distance1);
00314     status2 = range2.get_distance(&distance2);
00315     
00316     dist1 = format_dist(distance1);
00317     dist2 = format_dist(distance2);
00318     
00319     check_for_step(1, status1, dist1, led1, countdown1_triggered, countdown2_triggered, 
00320                     range1_triggered, range2_triggered, packet_queue);
00321     check_for_step(0, status2, dist2, led2, countdown2_triggered, countdown1_triggered, 
00322                     range2_triggered, range1_triggered, packet_queue);
00323 }
00324  
00325 // system put to sleep by waitForEvent() call and periodically interrupted
00326 // by Ticker to check for steps and packets to send
00327 int main(void)
00328 {
00329     range1.init_sensor(range1_addr);
00330     range2.init_sensor(range2_addr);
00331     
00332     optimize_transmissions(packet_queue);
00333     
00334     BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
00335     ble.init(bleInitComplete);
00336     
00337     ticker.attach(wakeup_event_cb, 0.1 * TIME_SCALE);
00338     
00339     while (ble.hasInitialized()) {
00340         ble.waitForEvent(); 
00341     }
00342 }