workss
Dependencies: mbed BLE_API nRF51822 VL53L0X
main.cpp
- Committer:
- vazbyte
- Date:
- 2019-03-24
- Revision:
- 48:78310b56de00
- Parent:
- 47:eeed074999ab
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(); } }