workss

Dependencies:   mbed BLE_API nRF51822 VL53L0X

Revision:
38:7035f67d8105
Parent:
37:5792986fea5d
Child:
39:067ba1a475a5
--- a/main.cpp	Fri Mar 15 20:02:49 2019 +0000
+++ b/main.cpp	Sat Mar 16 16:09:17 2019 +0000
@@ -35,7 +35,8 @@
  
 uint32_t distance1, distance2;
 int dist1, dist2, status1, status2;
-int timestamp = 0, current_time = 0;
+int packet_queue[4000] = {0};
+int packet_queue_index = 0;
 
 const static int cw = 20 / TIME_SCALE;
 const static int time_cycle = TIMESTAMP_FREQ / TIME_SCALE;
@@ -44,10 +45,12 @@
 bool countdown1_triggered = false, countdown2_triggered = false;
 bool step_in = false, step_out = false;
 bool range1_just_triggered = false, range2_just_triggered = false;
+bool master_connected = false;
  
 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; 
@@ -84,8 +87,8 @@
 
 void encode_bin(int direction, int time, int encoding[]) {
     time = time % maximum_time;
-    int bin[11] = {0};
-    dec_to_bin(time, bin); 
+    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)
@@ -109,12 +112,14 @@
         }
     }
     
-    int count = 10;
-    for (int i = 14; i >= 2; i--) {
-        if (i == 8 || i == 7) { }
-        else {
-            encoding[i] = bin[count];
-            count--;
+    if (!master_connected) { 
+        int count = 10;
+        for (int i = 14; i >= 2; i--) {
+            if (i == 8 || i == 7) { }
+            else {
+                encoding[i] = bin_timestamp[count];
+                count--;
+            }
         }
     }
 }
@@ -145,14 +150,55 @@
     printf("sent packet %i\n", p2);
 }
 
+void send_packet_from_queue(int current_index) {
+    double packet1 = packet_queue[current_index];
+    double packet2 = packet_queue[current_index + 1];
+    
+    int p1 = (int)packet1;
+    int p2 = (int)packet2;
+    
+    hrmCounter = p1;
+    hrService->updateHeartRate(hrmCounter);
+    printf("sent packet %i\n", p1);
+    hrmCounter = p2;
+    hrService->updateHeartRate(hrmCounter);
+    printf("sent packet %i\n", p2);
+}
+
+void enqueue_packet(int direction, int time, int queue[]) {     
+    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;
+    
+    packet_queue[packet_queue_index] = p1;
+    printf("enqueued packet %i\n", p1);
+    packet_queue[packet_queue_index + 1] = p2;
+    printf("enqueued packet %i\n", p2);
+    
+    ::packet_queue_index += 2;
+}
+
 void connectionCallback(const Gap::ConnectionCallbackParams_t *) {
     printf("Bluetooth connected at %i\n", current_time);
+    ::master_connected = true;
     send_packet(-1, current_time);
+    
+    int current_index = 0;
+    while (current_index < packet_queue_index) {
+        send_packet_from_queue(current_index);
+        current_index += 2;
+    }
+    ::packet_queue_index = 0;
 }
  
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *)
 {
     printf("Bluetooth disconnected at %i\n", current_time);
+    ::master_connected = false;
+    ::timestamp = 1;
     BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising();
 }
  
@@ -182,7 +228,7 @@
 void wakeup_event_cb() {
 
     led != led;
-    timestamp++;
+    ::timestamp++;
     ::current_time = timestamp / time_cycle;
     
     if (countdown1_triggered) {
@@ -214,7 +260,11 @@
                 countdown = cw;
             } else if (countdown2_triggered && !range1_just_triggered) {
                 printf("STEP IN DETECTED\n");
-                send_packet(1, current_time);
+                if (master_connected) {
+                    send_packet(1, 0);
+                } else {
+                    enqueue_packet(1, current_time, packet_queue);
+                }
 //                hrmCounter = 1;
 //                hrService->updateHeartRate(hrmCounter);
                 countdown2_triggered = false;
@@ -241,7 +291,11 @@
                 countdown = cw;
             } else if (countdown1_triggered && !range2_just_triggered) {
                 printf("STEP OUT DETECTED\n");
-                send_packet(0, current_time);
+                if (master_connected) {
+                    send_packet(1, 0);
+                } else {
+                    enqueue_packet(1, current_time, packet_queue);
+                }
 //                hrmCounter = 16;
 //                hrService->updateHeartRate(hrmCounter);
                 countdown1_triggered = false;