Diff: main.cpp

Revision:

5:dbb770470110

Parent:

4:cd99bf0e7502

--- a/main.cpp	Wed Oct 24 13:05:04 2018 +0000
+++ b/main.cpp	Wed Oct 24 14:56:25 2018 +0000
@@ -55,7 +55,7 @@
     if (whoami == 0x73) { // WHO_AM_I should always be 0x68
         pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
         pc.printf("MPU9250 is online...\n\r");
-        /*
+        
 //        lcd.clear();
 //        lcd.printString("MPU9250 is", 0, 0);
         sprintf(buffer, "0x%x", whoami);
@@ -79,17 +79,17 @@
         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));
-        if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
-        if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
-        if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
-        if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+//        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");
+//        if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
+//        if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
         wait(1);
     } else {
         pc.printf("Could not connect to MPU9250: \n\r");
@@ -130,9 +130,12 @@
             mpu9250.readMagData(magCount);  // Read the x/y/z adc values
             // Calculate the magnetometer values in milliGauss
             // Include factory calibration per data sheet and user environmental corrections
-            mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
-            my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
-            mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+//            mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
+//            my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+//            mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+                mx =( (float)magCount[0]*mRes*magCalibration[0] +364.061676 )*0.984126974;  // get actual magnetometer value, this depends on scale being set
+                my =( (float)magCount[1]*mRes*magCalibration[1] +142.1022415)*1.016393453;
+                mz =( (float)magCount[2]*mRes*magCalibration[2] +593.7275085)*4.110003018;
         }
 
         Now = t.read_us();
@@ -166,6 +169,7 @@
             pc.printf("mx = %f", mx);
             pc.printf(" my = %f", my);
             pc.printf(" mz = %f  mG\n\r", mz);
+//            pc.printf("%f\t%f\t%f\n", mx, my, mz);
 /*
             tempCount = mpu9250.readTempData();  // Read the adc values
             temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
@@ -200,11 +204,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
             roll  *= 180.0f / PI;
 
             pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-            pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+//            pc.printf("average rate = %f\n\r", (float) sumCount/sum);
 //    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
 //    lcd.printString(buffer, 0, 4);
 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);