cansat-d_2018
/
MPU6050_get_offset
MPU6050のオフセット値を測定し修正するプログラムです。
main.cpp@8:2b8cd80bea3b, 2018-11-23 (annotated)
- Committer:
- falconsyunya
- Date:
- Fri Nov 23 22:08:37 2018 +0000
- Revision:
- 8:2b8cd80bea3b
- Parent:
- 7:30613e31988e
- Child:
- 9:79c7f239b6a0
2018/11/24 7:08
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
falconsyunya | 0:4ba3fa671062 | 1 | #include "mbed.h" |
falconsyunya | 0:4ba3fa671062 | 2 | #include "MPU6050.h" |
falconsyunya | 0:4ba3fa671062 | 3 | |
falconsyunya | 0:4ba3fa671062 | 4 | DigitalOut myled(LED1); |
falconsyunya | 3:157b19421485 | 5 | MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置 |
falconsyunya | 0:4ba3fa671062 | 6 | |
falconsyunya | 7:30613e31988e | 7 | int main(void) { |
falconsyunya | 6:f1106d9e843c | 8 | int accel[3];//accelを3つの配列で定義。 |
falconsyunya | 8:2b8cd80bea3b | 9 | int biasx=0;//x補正値を定義。 |
falconsyunya | 8:2b8cd80bea3b | 10 | float biasxsum=0; |
falconsyunya | 8:2b8cd80bea3b | 11 | float biasxav=0; |
falconsyunya | 8:2b8cd80bea3b | 12 | int biasy=0;//y補正値を定義。 |
falconsyunya | 8:2b8cd80bea3b | 13 | float biasysum=0; |
falconsyunya | 8:2b8cd80bea3b | 14 | float biasyav=0; |
falconsyunya | 8:2b8cd80bea3b | 15 | int biasz=0;//z補正値を定義。 |
falconsyunya | 8:2b8cd80bea3b | 16 | float biaszsum=0; |
falconsyunya | 8:2b8cd80bea3b | 17 | float biaszav=0; |
falconsyunya | 8:2b8cd80bea3b | 18 | int x;//x軸方向の加速度。 |
falconsyunya | 8:2b8cd80bea3b | 19 | int y;//y軸方向の加速度。 |
falconsyunya | 7:30613e31988e | 20 | int z;//z軸方向の加速度。 |
falconsyunya | 8:2b8cd80bea3b | 21 | float X;//加速度の変換後の値。 |
falconsyunya | 8:2b8cd80bea3b | 22 | float Y;//加速度の変換後の値。 |
falconsyunya | 7:30613e31988e | 23 | float Z;//加速度の変換後の値。 |
falconsyunya | 8:2b8cd80bea3b | 24 | //Z軸方向の加速度バイアス平均値計算。 |
falconsyunya | 8:2b8cd80bea3b | 25 | for(int i=0;i<2000;i++){ |
falconsyunya | 8:2b8cd80bea3b | 26 | mpu.readAccelData(accel); |
falconsyunya | 8:2b8cd80bea3b | 27 | if(Z<9.797044){ |
falconsyunya | 8:2b8cd80bea3b | 28 | biasz++; |
falconsyunya | 8:2b8cd80bea3b | 29 | z = accel[2]+biasz;//z軸方向の加速度 |
falconsyunya | 8:2b8cd80bea3b | 30 | Z = z*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 31 | printf("%f\r\n",Z); |
falconsyunya | 8:2b8cd80bea3b | 32 | } |
falconsyunya | 8:2b8cd80bea3b | 33 | else{ |
falconsyunya | 8:2b8cd80bea3b | 34 | biasz--; |
falconsyunya | 8:2b8cd80bea3b | 35 | z = accel[2]+biasz;//z軸方向の加速度 |
falconsyunya | 8:2b8cd80bea3b | 36 | Z = z*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 37 | printf("%f\r\n",Z); |
falconsyunya | 8:2b8cd80bea3b | 38 | } |
falconsyunya | 8:2b8cd80bea3b | 39 | } |
falconsyunya | 8:2b8cd80bea3b | 40 | for(int i=0;i<100;i++){ |
falconsyunya | 8:2b8cd80bea3b | 41 | mpu.readAccelData(accel); |
falconsyunya | 8:2b8cd80bea3b | 42 | if(Z<9.797044){ |
falconsyunya | 8:2b8cd80bea3b | 43 | biasz++; |
falconsyunya | 8:2b8cd80bea3b | 44 | z = accel[2]+biasz; |
falconsyunya | 8:2b8cd80bea3b | 45 | Z = z*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 46 | printf("%f\r\n",Z); |
falconsyunya | 8:2b8cd80bea3b | 47 | } |
falconsyunya | 8:2b8cd80bea3b | 48 | else{ |
falconsyunya | 8:2b8cd80bea3b | 49 | biasz--; |
falconsyunya | 8:2b8cd80bea3b | 50 | z = accel[2]+biasz; |
falconsyunya | 8:2b8cd80bea3b | 51 | Z = z*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 52 | printf("%f\r\n",Z); |
falconsyunya | 8:2b8cd80bea3b | 53 | } |
falconsyunya | 8:2b8cd80bea3b | 54 | biaszsum = biaszsum + biasz; |
falconsyunya | 8:2b8cd80bea3b | 55 | } |
falconsyunya | 8:2b8cd80bea3b | 56 | biaszav = biaszsum/101; |
falconsyunya | 8:2b8cd80bea3b | 57 | printf("bias=%f\r\n",biaszav); |
falconsyunya | 8:2b8cd80bea3b | 58 | getchar(); |
falconsyunya | 8:2b8cd80bea3b | 59 | //Z軸方向の加速度バイアス計算終了。 |
falconsyunya | 8:2b8cd80bea3b | 60 | //X軸方向の加速度バイアス平均値計算。 |
falconsyunya | 8:2b8cd80bea3b | 61 | for(int i=0;i<2000;i++){ |
falconsyunya | 8:2b8cd80bea3b | 62 | mpu.readAccelData(accel); |
falconsyunya | 8:2b8cd80bea3b | 63 | if(X<9.797044){ |
falconsyunya | 8:2b8cd80bea3b | 64 | biasx++; |
falconsyunya | 8:2b8cd80bea3b | 65 | x = accel[0]+biasx; |
falconsyunya | 8:2b8cd80bea3b | 66 | X = x*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 67 | printf("%f\r\n",X); |
falconsyunya | 8:2b8cd80bea3b | 68 | } |
falconsyunya | 8:2b8cd80bea3b | 69 | else{ |
falconsyunya | 8:2b8cd80bea3b | 70 | biasx--; |
falconsyunya | 8:2b8cd80bea3b | 71 | x = accel[2]+biasx; |
falconsyunya | 8:2b8cd80bea3b | 72 | X = x*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 73 | printf("%f\r\n",X); |
falconsyunya | 8:2b8cd80bea3b | 74 | } |
falconsyunya | 8:2b8cd80bea3b | 75 | } |
falconsyunya | 8:2b8cd80bea3b | 76 | for(int i=0;i<100;i++){ |
falconsyunya | 8:2b8cd80bea3b | 77 | mpu.readAccelData(accel); |
falconsyunya | 8:2b8cd80bea3b | 78 | if(X<9.797044){ |
falconsyunya | 8:2b8cd80bea3b | 79 | biasx++; |
falconsyunya | 8:2b8cd80bea3b | 80 | x = accel[0]+biasx; |
falconsyunya | 8:2b8cd80bea3b | 81 | X = x*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 82 | printf("%f\r\n",X); |
falconsyunya | 8:2b8cd80bea3b | 83 | } |
falconsyunya | 8:2b8cd80bea3b | 84 | else{ |
falconsyunya | 8:2b8cd80bea3b | 85 | biasx--; |
falconsyunya | 8:2b8cd80bea3b | 86 | x = accel[0]+biasx; |
falconsyunya | 8:2b8cd80bea3b | 87 | X = x*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 88 | printf("%f\r\n",X); |
falconsyunya | 8:2b8cd80bea3b | 89 | } |
falconsyunya | 8:2b8cd80bea3b | 90 | biasxsum = biasxsum + biasx; |
falconsyunya | 8:2b8cd80bea3b | 91 | } |
falconsyunya | 8:2b8cd80bea3b | 92 | biasxav = biasxsum/101; |
falconsyunya | 8:2b8cd80bea3b | 93 | printf("bias=%f\r\n",biasxav); |
falconsyunya | 8:2b8cd80bea3b | 94 | getchar(); |
falconsyunya | 8:2b8cd80bea3b | 95 | //X軸方向の加速度バイアス計算終了。 |
falconsyunya | 8:2b8cd80bea3b | 96 | while(1){ |
falconsyunya | 1:958b581f6d21 | 97 | mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 |
falconsyunya | 8:2b8cd80bea3b | 98 | x = accel[0]+biasx;//x軸方向の加速度 |
falconsyunya | 8:2b8cd80bea3b | 99 | y = accel[1];//y軸方向の加速度 |
falconsyunya | 8:2b8cd80bea3b | 100 | z = accel[2]+biasz;//z軸方向の加速度 |
falconsyunya | 8:2b8cd80bea3b | 101 | X = x*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 102 | Y = y*0.000597964111328125; |
falconsyunya | 7:30613e31988e | 103 | Z = z*0.000597964111328125; |
falconsyunya | 8:2b8cd80bea3b | 104 | double a = X*X+Y*Y+Z*Z-95.982071137936; |
falconsyunya | 8:2b8cd80bea3b | 105 | printf("%lf %f %f %f\r\n",a,X,Y,Z); |
falconsyunya | 0:4ba3fa671062 | 106 | } |
falconsyunya | 8:2b8cd80bea3b | 107 | } |