hige dura
/
IMU
main.cpp@0:b61f317f452e, 2012-01-30 (annotated)
- Committer:
- higedura
- Date:
- Mon Jan 30 08:02:30 2012 +0000
- Revision:
- 0:b61f317f452e
- Child:
- 1:aca0c191fb1c
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
higedura | 0:b61f317f452e | 1 | // IMU for Sparkfun 9DOF Sensor Stick |
higedura | 0:b61f317f452e | 2 | |
higedura | 0:b61f317f452e | 3 | #include "ADXL345_I2C.h" |
higedura | 0:b61f317f452e | 4 | #include "ITG3200.h" |
higedura | 0:b61f317f452e | 5 | #include "HMC5883L.h" |
higedura | 0:b61f317f452e | 6 | |
higedura | 0:b61f317f452e | 7 | ADXL345_I2C accelerometer(p9, p10); |
higedura | 0:b61f317f452e | 8 | ITG3200 gyro(p9, p10); |
higedura | 0:b61f317f452e | 9 | HMC5883L compass(p9, p10); |
higedura | 0:b61f317f452e | 10 | Serial pc(USBTX, USBRX); |
higedura | 0:b61f317f452e | 11 | |
higedura | 0:b61f317f452e | 12 | #define N 3 |
higedura | 0:b61f317f452e | 13 | #define pi 3.14159265 |
higedura | 0:b61f317f452e | 14 | |
higedura | 0:b61f317f452e | 15 | double* RK4( double, double[N], double[N] ); |
higedura | 0:b61f317f452e | 16 | double* func( double[N], double[N] ); |
higedura | 0:b61f317f452e | 17 | |
higedura | 0:b61f317f452e | 18 | int main(){ |
higedura | 0:b61f317f452e | 19 | |
higedura | 0:b61f317f452e | 20 | float dt = 0.01; |
higedura | 0:b61f317f452e | 21 | float t = 0; |
higedura | 0:b61f317f452e | 22 | |
higedura | 0:b61f317f452e | 23 | pc.baud(921600); |
higedura | 0:b61f317f452e | 24 | |
higedura | 0:b61f317f452e | 25 | int bit_acc[N] = {0}; // Buffer of the accelerometer |
higedura | 0:b61f317f452e | 26 | int get_mag[N] = {0}; // Buffer of the compass |
higedura | 0:b61f317f452e | 27 | |
higedura | 0:b61f317f452e | 28 | double acc [N] = {0}; |
higedura | 0:b61f317f452e | 29 | double gyro_deg [N] = {0}; |
higedura | 0:b61f317f452e | 30 | double gyro_rad [N] = {0}; |
higedura | 0:b61f317f452e | 31 | int mag [N] = {0}; |
higedura | 0:b61f317f452e | 32 | |
higedura | 0:b61f317f452e | 33 | double ang_deg[N] = {0}; |
higedura | 0:b61f317f452e | 34 | double ang_rad[N] = {0}; |
higedura | 0:b61f317f452e | 35 | |
higedura | 0:b61f317f452e | 36 | double* p_ang; |
higedura | 0:b61f317f452e | 37 | |
higedura | 0:b61f317f452e | 38 | // *** Setting up accelerometer *** |
higedura | 0:b61f317f452e | 39 | // These are here to test whether any of the initialization fails. It will print the failure. |
higedura | 0:b61f317f452e | 40 | if (accelerometer.setPowerControl(0x00)){ |
higedura | 0:b61f317f452e | 41 | pc.printf("didn't intitialize power control\n\r"); |
higedura | 0:b61f317f452e | 42 | return 0; |
higedura | 0:b61f317f452e | 43 | } |
higedura | 0:b61f317f452e | 44 | // Full resolution, +/-16g, 4mg/LSB. |
higedura | 0:b61f317f452e | 45 | wait(.001); |
higedura | 0:b61f317f452e | 46 | |
higedura | 0:b61f317f452e | 47 | if(accelerometer.setDataFormatControl(0x0B)){ |
higedura | 0:b61f317f452e | 48 | pc.printf("didn't set data format\n\r"); |
higedura | 0:b61f317f452e | 49 | return 0; } |
higedura | 0:b61f317f452e | 50 | wait(.001); |
higedura | 0:b61f317f452e | 51 | |
higedura | 0:b61f317f452e | 52 | // 3.2kHz data rate. |
higedura | 0:b61f317f452e | 53 | if(accelerometer.setDataRate(ADXL345_3200HZ)){ |
higedura | 0:b61f317f452e | 54 | pc.printf("didn't set data rate\n\r"); |
higedura | 0:b61f317f452e | 55 | return 0; |
higedura | 0:b61f317f452e | 56 | } |
higedura | 0:b61f317f452e | 57 | wait(.001); |
higedura | 0:b61f317f452e | 58 | |
higedura | 0:b61f317f452e | 59 | if(accelerometer.setPowerControl(MeasurementMode)) { |
higedura | 0:b61f317f452e | 60 | pc.printf("didn't set the power control to measurement\n\r"); |
higedura | 0:b61f317f452e | 61 | return 0; |
higedura | 0:b61f317f452e | 62 | } |
higedura | 0:b61f317f452e | 63 | // *** Setting up accelerometer *** |
higedura | 0:b61f317f452e | 64 | |
higedura | 0:b61f317f452e | 65 | // *** Setting up gyro *** |
higedura | 0:b61f317f452e | 66 | gyro.setLpBandwidth(LPFBW_42HZ); |
higedura | 0:b61f317f452e | 67 | |
higedura | 0:b61f317f452e | 68 | // *** Setting up compass *** |
higedura | 0:b61f317f452e | 69 | compass.setDefault(); |
higedura | 0:b61f317f452e | 70 | |
higedura | 0:b61f317f452e | 71 | // Wait some time for all sensors (Need at least 5ms) |
higedura | 0:b61f317f452e | 72 | wait(0.1); |
higedura | 0:b61f317f452e | 73 | |
higedura | 0:b61f317f452e | 74 | pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r"); |
higedura | 0:b61f317f452e | 75 | pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r"); |
higedura | 0:b61f317f452e | 76 | |
higedura | 0:b61f317f452e | 77 | while(1){ |
higedura | 0:b61f317f452e | 78 | // Updating accelerometer and compass |
higedura | 0:b61f317f452e | 79 | accelerometer.getOutput(bit_acc); |
higedura | 0:b61f317f452e | 80 | compass.readData(get_mag); |
higedura | 0:b61f317f452e | 81 | |
higedura | 0:b61f317f452e | 82 | // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga]) |
higedura | 0:b61f317f452e | 83 | // Calibration YAL 9DOF acc : X+6, Y-12, Z+44 |
higedura | 0:b61f317f452e | 84 | // Calibration green acc : X+1, Y-18, Z+45 |
higedura | 0:b61f317f452e | 85 | acc[0] = ((int16_t)bit_acc[0]+1)*0.004; |
higedura | 0:b61f317f452e | 86 | acc[1] = ((int16_t)bit_acc[1]-18)*0.004; |
higedura | 0:b61f317f452e | 87 | acc[2] = ((int16_t)bit_acc[2]+45)*0.004; |
higedura | 0:b61f317f452e | 88 | // Calibration YAL 9DOF gyro : X+28, Y-45, Z+34 |
higedura | 0:b61f317f452e | 89 | // Calibration green gyro : X+135, Y-12, Z-53 |
higedura | 0:b61f317f452e | 90 | gyro_deg[0] = (gyro.getGyroX()+135)/14.375; |
higedura | 0:b61f317f452e | 91 | gyro_deg[1] = (gyro.getGyroY()-12)/14.375; |
higedura | 0:b61f317f452e | 92 | gyro_deg[2] = (gyro.getGyroZ()-48)/14.375; |
higedura | 0:b61f317f452e | 93 | // Calibration YAL 9DOF Compass:X, Y, Z |
higedura | 0:b61f317f452e | 94 | mag[0] = (int16_t)get_mag[0]; |
higedura | 0:b61f317f452e | 95 | mag[1] = (int16_t)get_mag[1]; |
higedura | 0:b61f317f452e | 96 | mag[2] = (int16_t)get_mag[2]; |
higedura | 0:b61f317f452e | 97 | |
higedura | 0:b61f317f452e | 98 | // Low pass filter for acc |
higedura | 0:b61f317f452e | 99 | for ( int i=0;i<N;i++ ){ if( -0.05<acc[i] && acc[i]<0.05 ){ acc[i]=0; } } |
higedura | 0:b61f317f452e | 100 | |
higedura | 0:b61f317f452e | 101 | // Low pass filter for gyro |
higedura | 0:b61f317f452e | 102 | for ( int i=0;i<N;i++ ){ if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i]=0; } } |
higedura | 0:b61f317f452e | 103 | |
higedura | 0:b61f317f452e | 104 | for ( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; } |
higedura | 0:b61f317f452e | 105 | |
higedura | 0:b61f317f452e | 106 | // RK4 (Prediction) |
higedura | 0:b61f317f452e | 107 | p_ang = RK4(dt,ang_rad,gyro_rad); |
higedura | 0:b61f317f452e | 108 | for ( int i=0;i<N;i++ ){ ang_rad[i] = *p_ang; p_ang = p_ang+1; } |
higedura | 0:b61f317f452e | 109 | |
higedura | 0:b61f317f452e | 110 | for ( int i=0;i<N;i++ ){ ang_deg[i] = ang_rad[i]*180/pi; } |
higedura | 0:b61f317f452e | 111 | //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_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]); |
higedura | 0:b61f317f452e | 112 | //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2]); |
higedura | 0:b61f317f452e | 113 | pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]); |
higedura | 0:b61f317f452e | 114 | |
higedura | 0:b61f317f452e | 115 | wait(dt); |
higedura | 0:b61f317f452e | 116 | t += dt; |
higedura | 0:b61f317f452e | 117 | |
higedura | 0:b61f317f452e | 118 | } |
higedura | 0:b61f317f452e | 119 | } |
higedura | 0:b61f317f452e | 120 | |
higedura | 0:b61f317f452e | 121 | |
higedura | 0:b61f317f452e | 122 | double* RK4( double dt, double y[N], double x[N] ){ |
higedura | 0:b61f317f452e | 123 | |
higedura | 0:b61f317f452e | 124 | double yBuf[N] = {0}; |
higedura | 0:b61f317f452e | 125 | double k[N][4] = {0}; |
higedura | 0:b61f317f452e | 126 | |
higedura | 0:b61f317f452e | 127 | double* p_y = y; |
higedura | 0:b61f317f452e | 128 | |
higedura | 0:b61f317f452e | 129 | double* pk1; |
higedura | 0:b61f317f452e | 130 | double* pk2; |
higedura | 0:b61f317f452e | 131 | double* pk3; |
higedura | 0:b61f317f452e | 132 | double* pk4; |
higedura | 0:b61f317f452e | 133 | |
higedura | 0:b61f317f452e | 134 | for ( int i=0;i<N;i++){ yBuf[i] = y[i]; } |
higedura | 0:b61f317f452e | 135 | pk1 = func (yBuf,x); |
higedura | 0:b61f317f452e | 136 | for ( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; } |
higedura | 0:b61f317f452e | 137 | |
higedura | 0:b61f317f452e | 138 | for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; } |
higedura | 0:b61f317f452e | 139 | pk2 = func (yBuf,x); |
higedura | 0:b61f317f452e | 140 | for ( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; } |
higedura | 0:b61f317f452e | 141 | |
higedura | 0:b61f317f452e | 142 | for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; } |
higedura | 0:b61f317f452e | 143 | pk3 = func (yBuf,x); |
higedura | 0:b61f317f452e | 144 | for ( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; } |
higedura | 0:b61f317f452e | 145 | |
higedura | 0:b61f317f452e | 146 | for ( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; } |
higedura | 0:b61f317f452e | 147 | pk4 = func (yBuf,x); |
higedura | 0:b61f317f452e | 148 | for ( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; } |
higedura | 0:b61f317f452e | 149 | |
higedura | 0:b61f317f452e | 150 | for ( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; } |
higedura | 0:b61f317f452e | 151 | |
higedura | 0:b61f317f452e | 152 | return p_y; |
higedura | 0:b61f317f452e | 153 | |
higedura | 0:b61f317f452e | 154 | } |
higedura | 0:b61f317f452e | 155 | |
higedura | 0:b61f317f452e | 156 | |
higedura | 0:b61f317f452e | 157 | double* func( double y[N], double x[N] ){ |
higedura | 0:b61f317f452e | 158 | |
higedura | 0:b61f317f452e | 159 | double f[N] = {0}; |
higedura | 0:b61f317f452e | 160 | double* p_f = f; |
higedura | 0:b61f317f452e | 161 | |
higedura | 0:b61f317f452e | 162 | f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]); |
higedura | 0:b61f317f452e | 163 | f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]); |
higedura | 0:b61f317f452e | 164 | f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]); |
higedura | 0:b61f317f452e | 165 | |
higedura | 0:b61f317f452e | 166 | return p_f; |
higedura | 0:b61f317f452e | 167 | |
higedura | 0:b61f317f452e | 168 | } |