hige dura
RK4_euler
Dependencies: FatFileSystem mbed
Fork of
RK4_euler
by 
hige dura
Diff: main.cpp
- Revision:
- 2:3ffce3e97527
- Parent:
- 1:29713f02de29
- Child:
- 3:5b192b38b3bb
--- a/main.cpp Sat Nov 26 15:01:52 2011 +0000
+++ b/main.cpp Wed Jan 25 05:25:29 2012 +0000
@@ -1,106 +1,99 @@
#include "ADXL345_I2C.h"
#include "ITG3200.h"
-#include "HMC5843.h"
+#include "HMC5883L.h"
ADXL345_I2C accelerometer(p9, p10);
ITG3200 gyro(p9, p10);
-HMC5843 compass(p9, p10);
+HMC5883L compass(p9, p10);
Serial pc(USBTX, USBRX);
+#define N 3
+
int main(){
+
+ int i = 0;
float dt = 0.1;
- float t = 0.0;
- pc.baud(115200);
- 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};
+ float t = 0;
+ pc.baud(9600);
+ int bitAcc[N] = {0}; // Buffer of the accelerometer
+ int getMag[N] = {0}; // Buffer of the compass
+ double Acc [N] = {0};
+ double Gyro [N] = {0};
+ int Mag [N] = {0};
- // *** Part of the accelerometer ***
- // These are here to test whether any of the initialization fails. It will print the failure
+ // *** Setting up accelerometer ***
+ // These are here to test whether any of the initialization fails. It will print the failure.
if (accelerometer.setPowerControl(0x00)){
- pc.printf("didn't intitialize power control\n");
- return 0; }
+ pc.printf("didn't intitialize power control\n\r");
+ return 0;
+ }
// Full resolution, +/-16g, 4mg/LSB.
wait(.001);
if(accelerometer.setDataFormatControl(0x0B)){
- pc.printf("didn't set data format\n");
+ pc.printf("didn't set data format\n\r");
return 0; }
wait(.001);
// 3.2kHz data rate.
if(accelerometer.setDataRate(ADXL345_3200HZ)){
- pc.printf("didn't set data rate\n");
- return 0; }
+ pc.printf("didn't set data rate\n\r");
+ return 0;
+ }
wait(.001);
if(accelerometer.setPowerControl(MeasurementMode)) {
- pc.printf("didn't set the power control to measurement\n");
- return 0; }
- // *** Part of the accelerometer ***
+ pc.printf("didn't set the power control to measurement\n\r");
+ return 0;
+ }
+ // *** Setting up accelerometer ***
+ // *** Setting up gyro ***
gyro.setLpBandwidth(LPFBW_42HZ);
-
- //Continuous mode, , 10Hz measurement rate.
- // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
+
+ // *** Setting up compass ***
compass.setDefault();
- //Wait some time(Need at least 5ms)
+ // Wait some time for all sensors (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");
+ pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r");
+ pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r");
while(1){
+ // Updating accelerometer and compass
accelerometer.getOutput(bitAcc);
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;
+
+ // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
+ // Calibration YAL 9DOF Acc:X+6, Y-12, Z+44
+ // Calibration green Acc:X+1, Y-18, Z+45
+ Acc[0] = ((int16_t)bitAcc[0])*0.004;
+ Acc[1] = ((int16_t)bitAcc[1])*0.004;
+ Acc[2] = ((int16_t)bitAcc[2])*0.004;
+ // Calibration YAL 9DOF Gyro:X+28, Y-45, Z+34
+ // Calibration green Gyro:X+135, Y-12, Z-53
+ Gyro[0] = (gyro.getGyroX())/14.375;
+ Gyro[1] = (gyro.getGyroY())/14.375;
+ Gyro[2] = (gyro.getGyroZ())/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];
+ Mag[0] = (int16_t)getMag[0];
+ Mag[1] = (int16_t)getMag[1];
+ Mag[2] = (int16_t)getMag[2];
+
+ // Low pass filter for acc
+ //for ( int i=0;i<N;i++ ){
+ // if( -0.05<Acc[i] && Acc[i]<0.05 ){ Acc[i]=0; }
+ //}
// 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; }
-
- // 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;
-
- // 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, %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];
- Gyro0[0] = Gyro1[0]; Gyro0[1] = Gyro1[1]; Gyro0[2] = Gyro1[2];
- Ang0[0] = Ang1[0]; Ang0[1] = Ang1[1]; Ang0[2] = Ang1[2];
+ //for ( int i=0;i<N;i++ ){
+ // if( -1.0<Gyro[i] && Gyro[i]<1.0 ){ Gyro[i]=0; }
+ //}
+
+ pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2], Mag[0], Mag[1], Mag[2]);
t += dt;
wait(dt);
}
-
-}
+ }