SparkFun_9DOF_SensorStick
Dependencies: FatFileSystem mbed
Fork of 9DOF_SensorStick by
Diff: main.cpp
- Revision:
- 1:29713f02de29
- Parent:
- 0:80d32420bc63
- Child:
- 2:3ffce3e97527
--- a/main.cpp Fri Nov 25 05:26:06 2011 +0000 +++ b/main.cpp Sat Nov 26 15:01:52 2011 +0000 @@ -1,22 +1,26 @@ #include "ADXL345_I2C.h" #include "ITG3200.h" +#include "HMC5843.h" ADXL345_I2C accelerometer(p9, p10); ITG3200 gyro(p9, p10); +HMC5843 compass(p9, p10); Serial pc(USBTX, USBRX); int main(){ float dt = 0.1; float t = 0.0; pc.baud(115200); - int bitAcc[3] = {0, 0, 0}; - double Acc [3] = {0, 0, 0}; - double Gyro0 [3] = {0, 0, 0}; - double Gyro1 [3] = {0, 0, 0}; - double Gyro2 [3] = {0, 0, 0}; - double Ang0 [3] = {0, 0, 0}; - double Ang1 [3] = {0, 0, 0}; - double Ang2 [3] = {0, 0, 0}; + int bitAcc[3] = {0, 0, 0}; + int getMag[3]; + double Acc [3] = {0, 0, 0}; + double Gyro0 [3] = {0, 0, 0}; + double Gyro1 [3] = {0, 0, 0}; + double Gyro2 [3] = {0, 0, 0}; + double Ang0 [3] = {0, 0, 0}; + double Ang1 [3] = {0, 0, 0}; + double Ang2 [3] = {0, 0, 0}; + int Mag1 [3] = {0, 0, 0}; // *** Part of the accelerometer *** // These are here to test whether any of the initialization fails. It will print the failure @@ -36,9 +40,7 @@ pc.printf("didn't set data rate\n"); return 0; } wait(.001); - - // Measurement mode. - + if(accelerometer.setPowerControl(MeasurementMode)) { pc.printf("didn't set the power control to measurement\n"); return 0; } @@ -46,33 +48,51 @@ gyro.setLpBandwidth(LPFBW_42HZ); - pc.printf("Starting ADXL345 and ITG3200 test...\n"); - //pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ\n"); - pc.printf(" Time GyroX GyroY GyroZ AngleX AngleY AngleZ\n"); + //Continuous mode, , 10Hz measurement rate. + // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA + compass.setDefault(); + + //Wait some time(Need at least 5ms) + wait(0.1); + + pc.printf("Starting ADXL345, ITG3200 and HMC5843 test...\n"); + pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n"); + //pc.printf(" Time GyroX GyroY GyroZ AngleX AngleY AngleZ\n"); while(1){ accelerometer.getOutput(bitAcc); - // Transfering units (Acc[g], Gyro[deg/s]) - Acc[0] = ((int16_t)bitAcc[0])*0.004; Acc[1] = ((int16_t)bitAcc[1])*0.004; Acc[2] = ((int16_t)bitAcc[2]+50)*0.004; - Gyro2[0] = (gyro.getGyroX()+30)/14.375; Gyro2[1] = (gyro.getGyroY()-28)/14.375; Gyro2[2] = (gyro.getGyroZ()-17)/14.375; + compass.readData(getMag); + // Transfering units (Acc[g], Gyro[deg/s], Compass[?]) + // Calibration YAL 9DOF Acc:X+2, Y-12, Z+44 + Acc[0] = ((int16_t)bitAcc[0]+2)*0.004; + Acc[1] = ((int16_t)bitAcc[1]-12)*0.004; + Acc[2] = ((int16_t)bitAcc[2]+44)*0.004; + // Calibration YAL 9DOF Gyro:X+26, Y-45, Z+34 + Gyro2[0] = (gyro.getGyroX()+26)/14.375; + Gyro2[1] = (gyro.getGyroY()-45)/14.375; + Gyro2[2] = (gyro.getGyroZ()+34)/14.375; + // Calibration YAL 9DOF Compass:X, Y, Z + Mag1[0] = (int16_t)getMag[0]; + Mag1[1] = (int16_t)getMag[1]; + Mag1[2] = (int16_t)getMag[2]; // Low pass filter for gyro - if( -1.0<Gyro2[0] && Gyro2[0]<1.0 ){ Gyro2[0]=0; } - if( -1.0<Gyro2[1] && Gyro2[1]<1.0 ){ Gyro2[1]=0; } - if( -1.0<Gyro2[2] && Gyro2[2]<1.0 ){ Gyro2[2]=0; } + //if( -1.0<Gyro2[0] && Gyro2[0]<1.0 ){ Gyro2[0]=0; } + //if( -1.0<Gyro2[1] && Gyro2[1]<1.0 ){ Gyro2[1]=0; } + //if( -1.0<Gyro2[2] && Gyro2[2]<1.0 ){ Gyro2[2]=0; } // Trapezoidal integration - Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2; - Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2; - Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2; + //Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2; + //Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2; + //Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2; // Simpson integration // Ang1[0] = Ang0[0]+(Gyro0[0]+4*Gyro1[0]+Gyro2[0])*dt/6; // Ang1[1] = Ang0[1]+(Gyro0[1]+4*Gyro1[1]+Gyro2[1])*dt/6; // Ang1[2] = Ang0[2]+(Gyro0[2]+4*Gyro1[2]+Gyro2[2])*dt/6; - //pc.printf("%6.1f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, Acc[0], Acc[1], Acc[2], Gyro1[0], Gyro1[1], Gyro1[2]); - pc.printf("%6.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]); + pc.printf("%6.1f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, Acc[0], Acc[1], Acc[2], Gyro1[0], Gyro1[1], Gyro1[2], Mag1[0], Mag1[1], Mag1[2]); + //pc.printf("%6.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]); //pc.printf("%6.1f, %6i, %6i, %6i, %7.2f, %7.2f, %7.2f\n\r", t, gyro.getGyroX()+32, gyro.getGyroY()-27, gyro.getGyroZ()-17, Ang1[0], Ang1[1], Ang1[2]); //pc.printf("%6.1f, %6i, %6i, %6i, %7.3f, %7.3f, %7.3f\n\r", t, (int16_t)bitAcc[0], (int16_t)bitAcc[1], (int16_t)bitAcc[2], Acc[0], Acc[1], Acc[2]); Gyro1[0] = Gyro2[0]; Gyro1[1] = Gyro2[1]; Gyro1[2] = Gyro2[2];