David k
Kalibrasi AVRG
Diff: main.cpp
- Revision:
- 6:b970c0e20295
- Parent:
- 4:cd99bf0e7502
- Child:
- 7:d4fd89e4f19d
--- a/main.cpp Wed Oct 24 13:05:04 2018 +0000
+++ b/main.cpp Mon Oct 29 10:56:02 2018 +0000
@@ -41,20 +41,21 @@
int main()
{
pc.baud(9600);
+// pc.printf("start");
//Set up I2C
i2c.frequency(400000); // use fast (400 kHz) I2C
- pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
+// pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
t.start();
// lcd.init();
// lcd.setBrightness(0.05);
// Read the WHO_AM_I register, this is a good test of communication
uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
- pc.printf("I AM 0x%x\n\r", whoami);
- pc.printf("I SHOULD BE 0x73\n\r");
+// pc.printf("I AM 0x%x\n\r", whoami);
+// pc.printf("I SHOULD BE 0x73\n\r");
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");
+// 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);
@@ -81,19 +82,19 @@
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");
- wait(1);
- } else {
- pc.printf("Could not connect to MPU9250: \n\r");
- pc.printf("%#x \n", whoami);
+// 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");
+// pc.printf("%#x \n", whoami);
// lcd.clear();
// lcd.printString("MPU9250", 0, 0);
@@ -107,9 +108,9 @@
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
@@ -130,9 +131,10 @@
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])+ 348.056595)*1.013699505; // get actual magnetometer value, this depends on scale being set
+ my = (((float)magCount[1]*mRes*magCalibration[1] - magbias[1])+ 129.145752)*0.992905726;
+ mz = (((float)magCount[2]*mRes*magCalibration[2] - magbias[2])+365.9966815)* 0.99367091;
+
}
Now = t.read_us();
@@ -163,9 +165,10 @@
pc.printf(" gy = %f", gy);
pc.printf(" gz = %f deg/s\n\r", gz);
*/
- pc.printf("mx = %f", mx);
- pc.printf(" my = %f", my);
- pc.printf(" mz = %f mG\n\r", mz);
+// 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
@@ -203,8 +206,8 @@
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("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+// 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);