Diff: source/main.cpp

Revision:

22:0d59828e0b66

Parent:

20:1998218bbb63

--- a/source/main.cpp	Tue Nov 27 17:01:37 2018 +0000
+++ b/source/main.cpp	Thu Feb 07 14:27:23 2019 +0000
@@ -1,3 +1,19 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2014 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
 #include <events/mbed_events.h>
 #include <math.h> 
 
@@ -8,174 +24,176 @@
 #include "ble/Gap.h"
 #include "ble/services/HeartRateService.h"
 
-DigitalOut Led0(p3);
-DigitalOut Led1(p4);
-DigitalOut Led2(p28);
-DigitalOut Led3(p29);
-DigitalOut Led4(p30);
-DigitalOut Led5(p31);
+DigitalOut Led0(p31);
+DigitalOut Led1(p30);
+DigitalOut Led2(p29);
+DigitalOut Led3(p28);
+DigitalOut Led4(p4);
+DigitalOut Led5(p3);
 DigitalOut Led6(p11);
 DigitalOut Led7(p12);
 DigitalOut Led8(p13);
 DigitalOut Led9(p14);
-DigitalOut Led10(p15);
-DigitalOut Led11(p16);
-DigitalOut Led12(p17);
-DigitalOut Led13(p18);
-DigitalOut Led14(p19);
-DigitalOut Led15(p20);
-DigitalOut Led16(p22);
-DigitalOut Led17(p23);
-DigitalOut Led18(p24);
-DigitalOut Led19(p25);
 
-DigitalIn pb(p8);
+//DigitalOut led1(LED1, 1);
 
-int USV_count = 1;
-int old_pb = 1;
-int new_pb;
-    
 const static char     DEVICE_NAME[] = "ProVaida";
 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
-short hrmCounter = 0;
-static HeartRateService* hrService;
-
-bool BLE_conn = false;
 
 int16_t destination[3];
+short hrmCounter = 0;
 double step_threshold = 14.45;
 
 double oldAcceleration = 0.0;
 int callback_cycles = 1; // used to be 4
 int step;
+int run_threshold = 6; // used to be 7
+int run_count = 0;
+
 int totalsteps = 0;
 
-int run_threshold = 5; // used to be 7
-int run_count = 0;
-
+static HeartRateService* hrService;
 MPU9250 mpu = MPU9250(P0_26, P0_27);
 
-static EventQueue eventQueue(16 * EVENTS_EVENT_SIZE);
+static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
 
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
 {
     BLE::Instance().gap().startAdvertising();
 }
 
-void updateSensorValue() {    
-    wait(.001);
-    new_pb = pb;
-    if ((new_pb == 0) && (old_pb == 1)) {
-        USV_count++;
-        printf("count: %i\n", USV_count);
-    }
-    if (USV_count % 2) {        
-        step = 0;
-        callback_cycles++;
+void updateSensorValue() {
+    step = 0;
+    callback_cycles++;
     
-        mpu.readAccelData(destination);
+    mpu.readAccelData(destination);
     
-        double acc_x = destination[0] / 1000.0;
-        double acc_y = destination[1] / 1000.0;
-        double acc_z = destination[2] / 1000.0;
+    double acc_x = destination[0] / 1000.0;
+    double acc_y = destination[1] / 1000.0;
+    double acc_z = destination[2] / 1000.0;
     
-        double sqr_acc_x = acc_x*acc_x;
-        double sqr_acc_y = acc_y*acc_y;
-        double sqr_acc_z = acc_z*acc_z;
-    
-        double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
-        double accel = sqrt(sum_acc);
+    double sqr_acc_x = acc_x*acc_x;
+    double sqr_acc_y = acc_y*acc_y;
+    double sqr_acc_z = acc_z*acc_z;
     
-        if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 3)) {
-            if (callback_cycles <= run_threshold) {
-                if (run_count >= 2) {
-                    step = 2;
-                }
-                else {
-                    step = 1;
-                    run_count++;
-                }
+    double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
+    double accel = sqrt(sum_acc);
+
+    //printf("calback cycles: " );
+    //printf("%i\n", callback_cycles);  
+     
+    if (accel < step_threshold && oldAcceleration >= step_threshold && (callback_cycles > 2)) {
+        if (callback_cycles <= run_threshold) {
+            if (run_count >= 2) {
+                step = 2;
             }
             else {
                 step = 1;
-                run_count = 0;    
+                run_count++;
             }
-            callback_cycles = 0;
+        }
+        else {
+            step = 1;
+            run_count = 0;    
         }
-        if (step != 0)
-        {
-            totalsteps++;                
-            Led0 = ((totalsteps >> 0) % 2);
-            Led1 = ((totalsteps >> 1) % 2);
-            Led2 = ((totalsteps >> 2) % 2);
-            Led3 = ((totalsteps >> 3) % 2);
-            Led4 = ((totalsteps >> 4) % 2);
-            Led5 = ((totalsteps >> 5) % 2);
-            Led6 = ((totalsteps >> 6) % 2);
-            Led7 = ((totalsteps >> 7) % 2);
-            Led8 = ((totalsteps >> 8) % 2);
-            Led9 = ((totalsteps >> 9) % 2);
-            Led10 = ((totalsteps >> 10) % 2);
-            Led11 = ((totalsteps >> 11) % 2);
-            Led12 = ((totalsteps >> 12) % 2);
-            Led13 = ((totalsteps >> 12) % 2);
-            Led14 = ((totalsteps >> 14) % 2);
-            Led15 = ((totalsteps >> 15) % 2);
-            Led16 = ((totalsteps >> 16) % 2);
-            Led17 = ((totalsteps >> 17) % 2);
-            Led18 = ((totalsteps >> 18) % 2);
-            Led19 = ((totalsteps >> 19) % 2);
-            if ((totalsteps >> 20) % 2 == 1) {
-                totalsteps = 0;
-            }   
-        }
-        oldAcceleration = accel;
+        callback_cycles = 0;
+    }
+    if (step != 0)
+    {
+        printf("STEP: " );
+        printf("%i\n", step);          
 
-    } else if (totalsteps != 0) {
-       totalsteps = 0;
-    }         
-    old_pb = new_pb;    
+        totalsteps++;        
+        
+        printf("totalSTEPs: " );
+        printf("%i\n", totalsteps);  
+        
+        Led0 = ((totalsteps >> 0) % 2);
+        Led1 = ((totalsteps >> 1) % 2);
+        Led2 = ((totalsteps >> 2) % 2);
+        Led3 = ((totalsteps >> 3) % 2);
+        Led4 = ((totalsteps >> 4) % 2);
+        Led5 = ((totalsteps >> 5) % 2);
+        Led6 = ((totalsteps >> 6) % 2);
+        Led7 = ((totalsteps >> 7) % 2);
+        Led8 = ((totalsteps >> 8) % 2);
+        Led9 = ((totalsteps >> 9) % 2);
+    }
     
-    if (BLE_conn) {
-        hrmCounter = (short) step;    
-        hrService->updateHeartRate(hrmCounter);
-    }
+    oldAcceleration = accel;
+    
+    hrmCounter = (short) step;
+    //printf("STEP: " );
+    //printf("%hu\n", hrmCounter);  
+    
+    hrService->updateHeartRate(hrmCounter);
 }
 
 void blinkCallback(void)
 {
+    //led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
     BLE &ble = BLE::Instance();
     if (ble.gap().getState().connected) {
         eventQueue.call(updateSensorValue);
     }
 }
 
-void onBleInitError(BLE &ble, ble_error_t error) {}
+/**
+ * This function is called when the ble initialization process has failed
+ */
+void onBleInitError(BLE &ble, ble_error_t error)
+{
+    /* Initialization error handling should go here */
+}
 
+void printMacAddress()
+{
+    /* Print out device MAC address to the console*/
+    Gap::AddressType_t addr_type;
+    Gap::Address_t address;
+    BLE::Instance().gap().getAddress(&addr_type, address);
+    printf("DEVICE MAC ADDRESS: ");
+    for (int i = 5; i >= 1; i--){
+        printf("%02x:", address[i]);
+    }
+    printf("%02x\r\n", address[0]);
+}
+
+/**
+ * Callback triggered when the ble initialization process has finished
+ */
 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
 {
     BLE&        ble   = params->ble;
     ble_error_t error = params->error;
 
-    if (error != BLE_ERROR_NONE) {        
+    if (error != BLE_ERROR_NONE) {
+        /* In case of error, forward the error handling to onBleInitError */
         onBleInitError(ble, error);
         return;
     }
 
+    /* Ensure that it is the default instance of BLE */
     if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
         return;
     }
 
     ble.gap().onDisconnection(disconnectionCallback);
 
+    /* Setup primary service */
     hrService = new HeartRateService(ble, hrmCounter, HeartRateService::LOCATION_FINGER);
 
+    /* Setup advertising */
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
-    ble.gap().setAdvertisingInterval(1000); 
+    ble.gap().setAdvertisingInterval(1000); /* 1000ms */
     ble.gap().startAdvertising();
+
+
+    printMacAddress();
+
 }
 
 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
@@ -184,27 +202,16 @@
 }
 
 int main()
-{   
-    
-    pb.mode(PullUp);
+{    
     mpu.initMPU9250();
     
-    while (1) {
-        updateSensorValue();
-        
-        BLE &ble = BLE::Instance();
-        ble.onEventsToProcess(scheduleBleEventsProcessing);
-        ble.init(bleInitComplete);
-        
-        if (ble.gap().getState().connected) {
-            BLE_conn = true;
-            //eventQueue.call(updateSensorValue);
-        }        
+    eventQueue.call_every(80, blinkCallback);
 
-        //eventQueue.dispatch_forever();
-        
-        wait(0.1);
-    }
+    BLE &ble = BLE::Instance();
+    ble.onEventsToProcess(scheduleBleEventsProcessing);
+    ble.init(bleInitComplete);
+
+    eventQueue.dispatch_forever();
 
     return 0;
 }