Diff: main.cpp

Revision:

2:3946867c4748

Parent:

1:c80b6c45e943

Child:

3:2e25f16c9fc3

--- a/main.cpp	Thu Apr 20 15:01:13 2017 +0000
+++ b/main.cpp	Thu Apr 20 15:30:45 2017 +0000
@@ -35,6 +35,7 @@
 float sum = 0;
 uint32_t sumCount = 0;
 char buffer[14];
+const uint16_t dly_ms = 1;    // delay for quat computing and printing
 
 MPU9250 mpu9250;
 
@@ -51,7 +52,8 @@
   wait(3);
   
   
-  pc.baud(115200);  
+  //pc.baud(115200);  
+  pc.baud(256000);
 
   //Set up I2C
   i2c.frequency(400000);  // use fast (400 kHz) I2C  
@@ -142,51 +144,41 @@
     
     
     // Pass gyro rate as rad/s
-    mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
-    //mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+    //mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+    mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
 
     delt_t = t.read_ms() - count;
     
-    if (delt_t > 20) {     
-    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]);   
-    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-    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 / PI;
-    yaw   *= 180.0f / PI; 
-    //yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
-    yaw   -= 3.8f; // Declination in Cork, Ireland in Oct 2016
-    roll  *= 180.0f / PI;
-
-    //pc.printf("Yaw, Pitch, Roll: %f %f %f\n", yaw, pitch, roll);
-    //pc.printf("%f\t%f\t%f\n", yaw, pitch, roll);
-    //pc.printf("average rate = %f\n", (float) sumCount/sum);
-
-    //pc.printf("%.3f\n", (36.0 + ms5611.getAltitude()));
+    if (delt_t > dly_ms) {     
+        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]);   
+        pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+        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 / PI;
+        yaw   *= 180.0f / PI; 
+        //yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+        yaw   -= 3.8f; // Declination in Cork, Ireland in Oct 2016
+        roll  *= 180.0f / PI;
 
-    pc.printf("%.2f %.2f %.2f %.2f %.2f %.2f\n", 
-    yaw,
-    pitch, 
-    roll, 
-    ax,
-    ay,
-    az  
-    //ms5611.getTemperature(),
-    //5611.getAltitude()
-    );
-       
-        
-    myled= !myled;
-    count = t.read_ms(); 
+        pc.printf("%.2f %.2f %.2f\n%.2f %.2f %.2f\n\n", 
+        yaw,
+        pitch, 
+        roll, 
+        ax,
+        ay,
+        az  
+        );
+               
+        myled= !myled;
+        count = t.read_ms(); 
 
-    if(count > 1<<21) {
-        t.start(); // start the timer over again if ~30 minutes has passed
-        count = 0;
-        deltat= 0;
-        lastUpdate = t.read_us();
+        if(count > 1<<21) {
+            t.start(); // start the timer over again if ~30 minutes has passed
+            count = 0;
+            deltat= 0;
+            lastUpdate = t.read_us();
+        }
+        sum = 0;
+        sumCount = 0; 
     }
-    sum = 0;
-    sumCount = 0; 
-}
-}
- 
- }
\ No newline at end of file
+} 
+}
\ No newline at end of file