Revision 3:49052c8d8e3c, committed 2015-11-03

Comitter:

akihiro0105

Date:

Tue Nov 03 13:59:45 2015 +0000

Parent:

2:3822b8c8bbe2

Child:

4:f3caa73e67f1

Commit message:

20151103

Changed in this revision

--- a/main.cpp	Tue Nov 03 13:59:07 2015 +0000
+++ b/main.cpp	Tue Nov 03 13:59:45 2015 +0000
@@ -7,7 +7,7 @@
 #define LSM9DS0_G   0x6A // Would be 0x6A if SDO_G is LOW
 
 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
-                               * it will have an impact on code-size and power consumption. */
+* it will have an impact on code-size and power consumption. */
 #if NEED_CONSOLE_OUTPUT
 Serial  pc(USBTX, USBRX);
 #define DEBUG(...) { pc.printf(__VA_ARGS__); }
@@ -41,7 +41,7 @@
 void updateServiceValues(void);
 
 void disconnectionCallback(Gap::Handle_t handle)
-{   
+{
     ble.startAdvertising();
 }
 
@@ -67,12 +67,12 @@
 int main(void)
 {
     //t.start();
-    
+
     //beta = sqrt(3.0f / 4.0f) * 3.14f * (40.0f / 180.0f);
-    
+
     dof.begin();
     //dof.calLSM9DS0(dof.gbias, dof.abias);
-    
+
     ble.init();
     ble.onDisconnection(disconnectionCallback);
     ble.onConnection(onConnectionCallback);
@@ -91,73 +91,67 @@
 
     ble.addService(htmService);
     //Change nRF51822/projectconfig.h L118,L119 500_50 1000_100
-    
+
     Ticker ticker;
     ticker.attach(periodicCallback, 0.1);
-    
-    for (;;)
-    {
+
+    for (;;) {
         updateServiceValues();
     }
 }
 
 void updateServiceValues(void)
 {
-      //dof.readGyro();
-      //float gx = dof.calcGyro(dof.gx) - dof.gbias[0];
-      //float gy = dof.calcGyro(dof.gy) - dof.gbias[1];
-      //float gz = dof.calcGyro(dof.gz) - dof.gbias[2];
-      dof.readMag();
-      //float mx = dof.calcMag(dof.mx);
-      //float my = dof.calcMag(dof.my);
-      //float mz = dof.calcMag(dof.mz);
-      dof.readAccel();
-      //float ax = dof.calcAccel(dof.ax) - dof.abias[0];
-      //float ay = dof.calcAccel(dof.ay) - dof.abias[1];
-      //float az = dof.calcAccel(dof.az) - dof.abias[2];
-      //MadgwickAHRSupdate(ax, ay, az, gx*3.14f/180.0f, gy*3.14f/180.0f, gz*3.14f/180.0f, mx, my, mz);
-      
-      //Now = t.read_us();
-      //deltat = ((Now - lastUpdate)/1000000.0f);
-      //lastUpdate = Now;
-      
-      //float yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
-      //float pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-      //float roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-      //pitch *= 180.0f / 3.14;
-      //yaw   *= 180.0f / 3.14; 
-      //yaw   -= 13.8;
-      //roll  *= 180.0f / 3.14;
-      
-      float yaw,roll,pitch;
-      float x=dof.calcAccel(dof.ax);
-      float y=dof.calcAccel(dof.ay);
-      float z=dof.calcAccel(dof.az);
-      pitch = atan2(x, sqrt((y * y) + (z * z)));
-      roll = atan2(y, sqrt((x * x) + (z * z)));
-      pitch *= 180.0 / 3.14;
-      roll *= 180.0 / 3.14;
-      //
-      if (dof.my > 0)
-      {
-          yaw = 90 - (atan((float)dof.mx / (float)dof.my) * (180 / 3.14));
-      }
-      else if (dof.my < 0)
-      {
-          yaw = - (atan((float)dof.mx / (float)dof.my) * (180 / 3.14));
-      }
-      else // dof.my = 0
-      {
-          if (dof.mx < 0) yaw = 180;
-          else yaw = 0;
-      }
-      
-      yaw = atan2(z, sqrt((x * x) + (y * y)));
-      yaw *= 180.0 / 3.14;
-      
-      memcpy(thermTempPayload, &pitch, 4);
-      memcpy(thermTempPayload+4, &roll, 4);
-      memcpy(thermTempPayload+8, &yaw, 4);
+    //dof.readGyro();
+    //float gx = dof.calcGyro(dof.gx) - dof.gbias[0];
+    //float gy = dof.calcGyro(dof.gy) - dof.gbias[1];
+    //float gz = dof.calcGyro(dof.gz) - dof.gbias[2];
+    dof.readMag();
+    //float mx = dof.calcMag(dof.mx);
+    //float my = dof.calcMag(dof.my);
+    //float mz = dof.calcMag(dof.mz);
+    dof.readAccel();
+    //float ax = dof.calcAccel(dof.ax) - dof.abias[0];
+    //float ay = dof.calcAccel(dof.ay) - dof.abias[1];
+    //float az = dof.calcAccel(dof.az) - dof.abias[2];
+    //MadgwickAHRSupdate(ax, ay, az, gx*3.14f/180.0f, gy*3.14f/180.0f, gz*3.14f/180.0f, mx, my, mz);
+
+    //Now = t.read_us();
+    //deltat = ((Now - lastUpdate)/1000000.0f);
+    //lastUpdate = Now;
+
+    //float yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+    //float pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+    //float roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+    //pitch *= 180.0f / 3.14;
+    //yaw   *= 180.0f / 3.14;
+    //yaw   -= 13.8;
+    //roll  *= 180.0f / 3.14;
+
+    float yaw,roll,pitch;
+    float x=dof.calcAccel(dof.ax);
+    float y=dof.calcAccel(dof.ay);
+    float z=dof.calcAccel(dof.az);
+    pitch = atan2(x, sqrt((y * y) + (z * z)));
+    roll = atan2(y, sqrt((x * x) + (z * z)));
+    pitch *= 180.0 / 3.14;
+    roll *= 180.0 / 3.14;
+    //
+    if (dof.my > 0) {
+        yaw = 90 - (atan((float)dof.mx / (float)dof.my) * (180 / 3.14));
+    } else if (dof.my < 0) {
+        yaw = - (atan((float)dof.mx / (float)dof.my) * (180 / 3.14));
+    } else { // dof.my = 0
+        if (dof.mx < 0) yaw = 180;
+        else yaw = 0;
+    }
+
+    yaw = atan2(z, sqrt((x * x) + (y * y)));
+    yaw *= 180.0 / 3.14;
+
+    memcpy(thermTempPayload, &pitch, 4);
+    memcpy(thermTempPayload+4, &roll, 4);
+    memcpy(thermTempPayload+8, &yaw, 4);
 }
 
 /*void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {