Diff: main.cpp

Revision:

2:d4bd9ff10e8e

Parent:

1:5609c1795245

Child:

3:5bfa7291c639

--- a/main.cpp	Tue Feb 20 11:06:35 2018 +0000
+++ b/main.cpp	Fri Mar 23 16:25:17 2018 +0000
@@ -220,26 +220,26 @@
     
         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
         mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
-        //pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);  
-        //pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);  
-        //pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);  
-        //pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);  
-        //pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);  
-        //pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);  
+        pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);  
+        pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);  
+        pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);  
+        pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);  
+        pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);  
+        pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);  
         mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
-        //pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
-        //pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
-        //pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
-        //pc.printf("x accel bias = %f\n\r", accelBias[0]);
-        //pc.printf("y accel bias = %f\n\r", accelBias[1]);
-        //pc.printf("z accel bias = %f\n\r", accelBias[2]);
+        pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
+        pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
+        pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
+        pc.printf("x accel bias = %f\n\r", accelBias[0]);
+        pc.printf("y accel bias = %f\n\r", accelBias[1]);
+        pc.printf("z accel bias = %f\n\r", accelBias[2]);
         wait(2);
         mpu9250.initMPU9250(); 
-        //pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+        pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
         mpu9250.initAK8963(magCalibration);
-        //pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
-        //pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
-        //pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
+        pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+        pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
+        pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
         
         if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
         if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
@@ -260,13 +260,14 @@
             mpu9250.getAres(); // Get accelerometer sensitivity
             mpu9250.getGres(); // Get gyro sensitivity
             mpu9250.getMres(); // Get magnetometer sensitivity
-            //pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
-            //pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
-            //pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+            pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+            pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+            pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
             magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
             magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
             magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
 
+
             while(1) 
             {
                 // If intPin goes high, all data registers have new data
@@ -306,13 +307,13 @@
             //}
     
             //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);
 
             //Serial print and/or display at 0.5 s rate independent of data rates
             delt_t = t.read_ms() - count;
     
-            if (delt_t > 50) 
+            if (delt_t > 10) 
             { // update LCD once per half-second independent of read rate
 
                 //pc.printf("ax = %f", 1000*ax); 
@@ -351,10 +352,11 @@
                 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   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+                yaw   += 0.8f; 
                 roll  *= 180.0f / PI;
 
-                pc.printf("%f   %f  %f  %f \n\r",roll, pitch, yaw, origin);
+                pc.printf("%f  %f  %f  %f \n\r",roll, pitch, yaw,origin);
                 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
                 //sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
                 //lcd.printString(buffer, 0, 4);