hige dura
RK4_euler
Dependencies: FatFileSystem mbed
Fork of
RK4_euler
by 
hige dura
main.cpp@1:29713f02de29, 2011-11-26 (annotated)
- Committer:
- higedura
- Date:
- Sat Nov 26 15:01:52 2011 +0000
- Revision:
- 1:29713f02de29
- Parent:
- 0:80d32420bc63
- Child:
- 2:3ffce3e97527
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| higedura | 0:80d32420bc63 | 1 | #include "ADXL345_I2C.h" |
| higedura | 0:80d32420bc63 | 2 | #include "ITG3200.h" |
| higedura | 1:29713f02de29 | 3 | #include "HMC5843.h" |
| higedura | 0:80d32420bc63 | 4 | |
| higedura | 0:80d32420bc63 | 5 | ADXL345_I2C accelerometer(p9, p10); |
| higedura | 0:80d32420bc63 | 6 | ITG3200 gyro(p9, p10); |
| higedura | 1:29713f02de29 | 7 | HMC5843 compass(p9, p10); |
| higedura | 0:80d32420bc63 | 8 | Serial pc(USBTX, USBRX); |
| higedura | 0:80d32420bc63 | 9 | |
| higedura | 0:80d32420bc63 | 10 | int main(){ |
| higedura | 0:80d32420bc63 | 11 | float dt = 0.1; |
| higedura | 0:80d32420bc63 | 12 | float t = 0.0; |
| higedura | 0:80d32420bc63 | 13 | pc.baud(115200); |
| higedura | 1:29713f02de29 | 14 | int bitAcc[3] = {0, 0, 0}; |
| higedura | 1:29713f02de29 | 15 | int getMag[3]; |
| higedura | 1:29713f02de29 | 16 | double Acc [3] = {0, 0, 0}; |
| higedura | 1:29713f02de29 | 17 | double Gyro0 [3] = {0, 0, 0}; |
| higedura | 1:29713f02de29 | 18 | double Gyro1 [3] = {0, 0, 0}; |
| higedura | 1:29713f02de29 | 19 | double Gyro2 [3] = {0, 0, 0}; |
| higedura | 1:29713f02de29 | 20 | double Ang0 [3] = {0, 0, 0}; |
| higedura | 1:29713f02de29 | 21 | double Ang1 [3] = {0, 0, 0}; |
| higedura | 1:29713f02de29 | 22 | double Ang2 [3] = {0, 0, 0}; |
| higedura | 1:29713f02de29 | 23 | int Mag1 [3] = {0, 0, 0}; |
| higedura | 0:80d32420bc63 | 24 | |
| higedura | 0:80d32420bc63 | 25 | // *** Part of the accelerometer *** |
| higedura | 0:80d32420bc63 | 26 | // These are here to test whether any of the initialization fails. It will print the failure |
| higedura | 0:80d32420bc63 | 27 | if (accelerometer.setPowerControl(0x00)){ |
| higedura | 0:80d32420bc63 | 28 | pc.printf("didn't intitialize power control\n"); |
| higedura | 0:80d32420bc63 | 29 | return 0; } |
| higedura | 0:80d32420bc63 | 30 | // Full resolution, +/-16g, 4mg/LSB. |
| higedura | 0:80d32420bc63 | 31 | wait(.001); |
| higedura | 0:80d32420bc63 | 32 | |
| higedura | 0:80d32420bc63 | 33 | if(accelerometer.setDataFormatControl(0x0B)){ |
| higedura | 0:80d32420bc63 | 34 | pc.printf("didn't set data format\n"); |
| higedura | 0:80d32420bc63 | 35 | return 0; } |
| higedura | 0:80d32420bc63 | 36 | wait(.001); |
| higedura | 0:80d32420bc63 | 37 | |
| higedura | 0:80d32420bc63 | 38 | // 3.2kHz data rate. |
| higedura | 0:80d32420bc63 | 39 | if(accelerometer.setDataRate(ADXL345_3200HZ)){ |
| higedura | 0:80d32420bc63 | 40 | pc.printf("didn't set data rate\n"); |
| higedura | 0:80d32420bc63 | 41 | return 0; } |
| higedura | 0:80d32420bc63 | 42 | wait(.001); |
| higedura | 1:29713f02de29 | 43 | |
| higedura | 0:80d32420bc63 | 44 | if(accelerometer.setPowerControl(MeasurementMode)) { |
| higedura | 0:80d32420bc63 | 45 | pc.printf("didn't set the power control to measurement\n"); |
| higedura | 0:80d32420bc63 | 46 | return 0; } |
| higedura | 0:80d32420bc63 | 47 | // *** Part of the accelerometer *** |
| higedura | 0:80d32420bc63 | 48 | |
| higedura | 0:80d32420bc63 | 49 | gyro.setLpBandwidth(LPFBW_42HZ); |
| higedura | 0:80d32420bc63 | 50 | |
| higedura | 1:29713f02de29 | 51 | //Continuous mode, , 10Hz measurement rate. |
| higedura | 1:29713f02de29 | 52 | // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA |
| higedura | 1:29713f02de29 | 53 | compass.setDefault(); |
| higedura | 1:29713f02de29 | 54 | |
| higedura | 1:29713f02de29 | 55 | //Wait some time(Need at least 5ms) |
| higedura | 1:29713f02de29 | 56 | wait(0.1); |
| higedura | 1:29713f02de29 | 57 | |
| higedura | 1:29713f02de29 | 58 | pc.printf("Starting ADXL345, ITG3200 and HMC5843 test...\n"); |
| higedura | 1:29713f02de29 | 59 | pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n"); |
| higedura | 1:29713f02de29 | 60 | //pc.printf(" Time GyroX GyroY GyroZ AngleX AngleY AngleZ\n"); |
| higedura | 0:80d32420bc63 | 61 | |
| higedura | 0:80d32420bc63 | 62 | while(1){ |
| higedura | 0:80d32420bc63 | 63 | accelerometer.getOutput(bitAcc); |
| higedura | 1:29713f02de29 | 64 | compass.readData(getMag); |
| higedura | 1:29713f02de29 | 65 | // Transfering units (Acc[g], Gyro[deg/s], Compass[?]) |
| higedura | 1:29713f02de29 | 66 | // Calibration YAL 9DOF Acc:X+2, Y-12, Z+44 |
| higedura | 1:29713f02de29 | 67 | Acc[0] = ((int16_t)bitAcc[0]+2)*0.004; |
| higedura | 1:29713f02de29 | 68 | Acc[1] = ((int16_t)bitAcc[1]-12)*0.004; |
| higedura | 1:29713f02de29 | 69 | Acc[2] = ((int16_t)bitAcc[2]+44)*0.004; |
| higedura | 1:29713f02de29 | 70 | // Calibration YAL 9DOF Gyro:X+26, Y-45, Z+34 |
| higedura | 1:29713f02de29 | 71 | Gyro2[0] = (gyro.getGyroX()+26)/14.375; |
| higedura | 1:29713f02de29 | 72 | Gyro2[1] = (gyro.getGyroY()-45)/14.375; |
| higedura | 1:29713f02de29 | 73 | Gyro2[2] = (gyro.getGyroZ()+34)/14.375; |
| higedura | 1:29713f02de29 | 74 | // Calibration YAL 9DOF Compass:X, Y, Z |
| higedura | 1:29713f02de29 | 75 | Mag1[0] = (int16_t)getMag[0]; |
| higedura | 1:29713f02de29 | 76 | Mag1[1] = (int16_t)getMag[1]; |
| higedura | 1:29713f02de29 | 77 | Mag1[2] = (int16_t)getMag[2]; |
| higedura | 0:80d32420bc63 | 78 | |
| higedura | 0:80d32420bc63 | 79 | // Low pass filter for gyro |
| higedura | 1:29713f02de29 | 80 | //if( -1.0<Gyro2[0] && Gyro2[0]<1.0 ){ Gyro2[0]=0; } |
| higedura | 1:29713f02de29 | 81 | //if( -1.0<Gyro2[1] && Gyro2[1]<1.0 ){ Gyro2[1]=0; } |
| higedura | 1:29713f02de29 | 82 | //if( -1.0<Gyro2[2] && Gyro2[2]<1.0 ){ Gyro2[2]=0; } |
| higedura | 0:80d32420bc63 | 83 | |
| higedura | 0:80d32420bc63 | 84 | // Trapezoidal integration |
| higedura | 1:29713f02de29 | 85 | //Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2; |
| higedura | 1:29713f02de29 | 86 | //Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2; |
| higedura | 1:29713f02de29 | 87 | //Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2; |
| higedura | 0:80d32420bc63 | 88 | |
| higedura | 0:80d32420bc63 | 89 | // Simpson integration |
| higedura | 0:80d32420bc63 | 90 | // Ang1[0] = Ang0[0]+(Gyro0[0]+4*Gyro1[0]+Gyro2[0])*dt/6; |
| higedura | 0:80d32420bc63 | 91 | // Ang1[1] = Ang0[1]+(Gyro0[1]+4*Gyro1[1]+Gyro2[1])*dt/6; |
| higedura | 0:80d32420bc63 | 92 | // Ang1[2] = Ang0[2]+(Gyro0[2]+4*Gyro1[2]+Gyro2[2])*dt/6; |
| higedura | 0:80d32420bc63 | 93 | |
| higedura | 1:29713f02de29 | 94 | 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]); |
| higedura | 1:29713f02de29 | 95 | //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]); |
| higedura | 0:80d32420bc63 | 96 | //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]); |
| higedura | 0:80d32420bc63 | 97 | //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]); |
| higedura | 0:80d32420bc63 | 98 | Gyro1[0] = Gyro2[0]; Gyro1[1] = Gyro2[1]; Gyro1[2] = Gyro2[2]; |
| higedura | 0:80d32420bc63 | 99 | Gyro0[0] = Gyro1[0]; Gyro0[1] = Gyro1[1]; Gyro0[2] = Gyro1[2]; |
| higedura | 0:80d32420bc63 | 100 | Ang0[0] = Ang1[0]; Ang0[1] = Ang1[1]; Ang0[2] = Ang1[2]; |
| higedura | 0:80d32420bc63 | 101 | |
| higedura | 0:80d32420bc63 | 102 | t += dt; |
| higedura | 0:80d32420bc63 | 103 | wait(dt); |
| higedura | 0:80d32420bc63 | 104 | } |
| higedura | 0:80d32420bc63 | 105 | |
| higedura | 0:80d32420bc63 | 106 | } |