Diff: source/main.cpp

Revision:

3:a33709dad06c

Parent:

2:1957a4985d6e

Child:

4:af0530752b76

--- a/source/main.cpp	Sun Oct 28 13:45:26 2018 +0000
+++ b/source/main.cpp	Sun Oct 28 14:10:06 2018 +0000
@@ -15,6 +15,7 @@
  */
 
 #include <events/mbed_events.h>
+#include <math.h> 
 #include <mbed.h>
 #include "MPU9250.h"
 #include "ble/BLE.h"
@@ -27,7 +28,7 @@
 const static char     DEVICE_NAME[] = "ProVaida";
 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE};
 
-uint8_t hrmCounter = 100;
+uint8_t hrmCounter = 0;
 static HeartRateService* hrService;
 
 static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
@@ -42,6 +43,21 @@
 //    if (hrmCounter > 175) {
 //        hrmCounter = 100;
 //    }
+    MPU9250 mpu = MPU9250(p26, p27);
+    int16_t * destination;
+    mpu.readAccelData(destination);
+    
+    uint8_t acc_x = destination[0];
+    uint8_t acc_y = destination[1];
+    uint8_t acc_z = destination[2];
+    
+    uint8_t sqr_acc_x = acc_x*acc_x;
+    uint8_t sqr_acc_y = acc_y*acc_y;
+    uint8_t sqr_acc_z = acc_z*acc_z;
+    
+    double sum_acc = sqr_acc_x + sqr_acc_y + sqr_acc_z;
+    
+    hrmCounter = sqrt(sum_acc);
 
     hrService->updateHeartRate(hrmCounter);
 }
@@ -121,7 +137,7 @@
 {
     MPU9250 mpu = MPU9250(p26, p27);
     mpu.initMPU9250();
-    eventQueue.call_every(500, blinkCallback);
+    eventQueue.call_every(100, blinkCallback);
 
     BLE &ble = BLE::Instance();
     ble.onEventsToProcess(scheduleBleEventsProcessing);