cansat-d_2018
/
MPU6050_get_offset
MPU6050のオフセット値を測定し修正するプログラムです。
main.cpp
- Committer:
- falconsyunya
- Date:
- 2018-11-23
- Revision:
- 9:79c7f239b6a0
- Parent:
- 8:2b8cd80bea3b
File content as of revision 9:79c7f239b6a0:
#include "mbed.h" #include "MPU6050.h" DigitalOut myled(LED1); MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置 int main(void) { int accel[3];//accelを3つの配列で定義。 int biasx=0;//x補正値を定義。 float biasxsum=0; float biasxav=0; int biasy=0;//y補正値を定義。 float biasysum=0; float biasyav=0; int biasz=0;//z補正値を定義。 float biaszsum=0; float biaszav=0; int x;//x軸方向の加速度。 int y;//y軸方向の加速度。 int z;//z軸方向の加速度。 float X;//加速度の変換後の値。 float Y;//加速度の変換後の値。 float Z;//加速度の変換後の値。 //Z軸方向の加速度バイアス平均値計算。 for(int i=0;i<2000;i++){ mpu.readAccelData(accel); if(Z<9.797044){ biasz++; z = accel[2]+biasz;//z軸方向の加速度 Z = z*0.000597964111328125; printf("%f\r\n",Z); } else{ biasz--; z = accel[2]+biasz;//z軸方向の加速度 Z = z*0.000597964111328125; printf("%f\r\n",Z); } } for(int i=0;i<100;i++){ mpu.readAccelData(accel); if(Z<9.797044){ biasz++; z = accel[2]+biasz; Z = z*0.000597964111328125; printf("%f\r\n",Z); } else{ biasz--; z = accel[2]+biasz; Z = z*0.000597964111328125; printf("%f\r\n",Z); } biaszsum = biaszsum + biasz; } biaszav = biaszsum/101; printf("bias=%f\r\n",biaszav); getchar(); //Z軸方向の加速度バイアス計算終了。 //X軸方向の加速度バイアス平均値計算。 for(int i=0;i<2000;i++){ mpu.readAccelData(accel); if(X<9.797044){ biasx++; x = accel[0]+biasx; X = x*0.000597964111328125; printf("%f\r\n",X); } else{ biasx--; x = accel[2]+biasx; X = x*0.000597964111328125; printf("%f\r\n",X); } } for(int i=0;i<100;i++){ mpu.readAccelData(accel); if(X<9.797044){ biasx++; x = accel[0]+biasx; X = x*0.000597964111328125; printf("%f\r\n",X); } else{ biasx--; x = accel[0]+biasx; X = x*0.000597964111328125; printf("%f\r\n",X); } biasxsum = biasxsum + biasx; } biasxav = biasxsum/101; printf("bias=%f\r\n",biasxav); getchar(); //X軸方向の加速度バイアス計算終了。 //Y軸方向の加速度バイアス平均値計算。 for(int i=0;i<2000;i++){ mpu.readAccelData(accel); if(Y<9.797044){ biasy++; y = accel[0]+biasy; Y = y*0.000597964111328125; printf("%f\r\n",Y); } else{ biasy--; y = accel[2]+biasy; Y = y*0.000597964111328125; printf("%f\r\n",Y); } } for(int i=0;i<100;i++){ mpu.readAccelData(accel); if(Y<9.797044){ biasy++; y = accel[0]+biasy; Y = y*0.000597964111328125; printf("%f\r\n",Y); } else{ biasy--; y = accel[0]+biasy; Y = y*0.000597964111328125; printf("%f\r\n",Y); } biasysum = biasysum + biasy; } biasyav = biasysum/101; printf("bias=%f\r\n",biasyav); getchar(); //X軸方向の加速度バイアス計算終了。 while(1){ mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 x = accel[0]+biasx;//x軸方向の加速度 y = accel[1]+biasy;//y軸方向の加速度 z = accel[2]+biasz;//z軸方向の加速度 X = x*0.000597964111328125; Y = y*0.000597964111328125; Z = z*0.000597964111328125; double a = X*X+Y*Y+Z*Z-95.982071137936; printf("%lf %f %f %f\r\n",a,X,Y,Z); } }