Diff: aconnoHelpers/aconnoHelpers.cpp

Revision:

4:cb3513aa9814

Parent:

3:b62a73dc76b7

Child:

5:6d20ad2a69ba

diff -r b62a73dc76b7 -r cb3513aa9814 aconnoHelpers/aconnoHelpers.cpp
--- a/aconnoHelpers/aconnoHelpers.cpp	Fri Dec 14 13:33:03 2018 +0100
+++ b/aconnoHelpers/aconnoHelpers.cpp	Tue Jan 08 13:06:07 2019 +0000
@@ -15,6 +15,8 @@
 #include "MPL115A1.h"
 #include "acd_nrf52_saadc.h"
 #include "aconnoConfig.h"
+#include <iostream>
+#include <cmath>
 
 NRF52_SAADC analogIn;
 extern DigitalOut lightPower;
@@ -120,15 +122,48 @@
     memsMagnetometerInit /= CALIBRATION_STEPS;
 }
 
+float calculateDegree(float accX, float accY, float accZ){
+    
+    float degree;
+    if (accX != 0.0f && accY != 0.0f && accZ != 0.0f) {
+
+        float dnum = sqrt(accY * accY + accZ * accZ);
+        degree = atan(accX / dnum);
+
+        if ((accX < 0 && accY >= 0)) {
+            degree = 360 + degree;
+        }
+        if (accX >= 0 && accY < 0)
+            degree = 180 - degree;
+
+        if (accX < 0 && accY < 0)
+            degree = 180 - degree;
+
+           //cout<<"degree: " << degree;
+           
+        }
+    return degree;
+    
+}
 void updateData()
 {
-    static uint8_t advertisementType = 0;
+    //static uint8_t advertisementType = 0;
     int16_t temp_acc[3];
     BLE &ble = BLE::Instance();
 
     float result;
-
-    if(advertisementType < 1)
+    mems.startAccelerometer();
+   // advertisementPacket.type = 0x00;
+    //Added for Angular sensa
+     readAccelerometer((vector3_s *)temp_acc);
+       float degreeValue = calculateDegree( (float) temp_acc[1],
+        (float) temp_acc[0],
+         (float) temp_acc[2]);
+          
+        cout<<"degree: " << degreeValue;  
+        advertisementPacket.degree = degreeValue;  
+        
+    /*if(advertisementType < 1)
     {
         power = 1;
         mems.startAccelerometer();
@@ -139,10 +174,17 @@
         readGyroscope((vector3_s *)advertisementPacket.gyroscope);
         readAccelerometer((vector3_s *)temp_acc);
         readMagnetometer((vector3_s *)advertisementPacket.magnetometer);
-        advertisementPacket.acc_lsb_value = (0xF9E);//(0x3D80);
-        advertisementPacket.accelerometer[0] = temp_acc[1];
-        advertisementPacket.accelerometer[1] = temp_acc[0];
-        advertisementPacket.accelerometer[2] = temp_acc[2];
+        //advertisementPacket.acc_lsb_value = (0xF9E);//(0x3D80);
+        //advertisementPacket.accelerometer[0] = temp_acc[1];
+        //advertisementPacket.accelerometer[1] = temp_acc[0];
+        //advertisementPacket.accelerometer[2] = temp_acc[2];
+        
+        float degreeValue = calculateDegree( (float) temp_acc[1],
+        (float) temp_acc[0],
+         (float) temp_acc[2]);
+          
+        cout<<"degree: " << degreeValue;  
+        advertisementPacket.degree = degreeValue;  
         // ^--- That's in ug cuz MSB is 1
     }
     else
@@ -167,7 +209,7 @@
         advertisementPacket.pressure    = mpl115a1.getPressure();
         advertisementPacket.battery     = getBattery();
     }
-
+    */
     SPI dummySpi(SPI_MOSI, SPI_MISO, SPI_SCLK);
     power = 0;
     shdn = 0;
@@ -175,9 +217,9 @@
     lightPower = 0;
     NRF_SAADC->ENABLE = 0;
 
-    if(++advertisementType > 2) advertisementType = 0;    
+    //if(++advertisementType > 2) advertisementType = 0;    
     adv_data = ble.gap().getAdvertisingPayload();
     adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, 
                 (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
     ble.gap().setAdvertisingPayload(adv_data);
-}
\ No newline at end of file
+}