cansat-d_2018
/
MPU6050_get_offset
MPU6050のオフセット値を測定し修正するプログラムです。
Diff: main.cpp
- Revision:
- 4:d2bb399c7c8c
- Parent:
- 3:157b19421485
- Child:
- 5:8bc5cef21589
diff -r 157b19421485 -r d2bb399c7c8c main.cpp --- a/main.cpp Mon Oct 29 12:22:57 2018 +0000 +++ b/main.cpp Fri Nov 09 21:03:51 2018 +0000 @@ -5,13 +5,35 @@ MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置 int main() { + float filterCoefficient = 0.3; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。 + float lowpassValue = 0; + float highpassValue = 0; + float speed = 0;//加速度時から算出した速度 + float oldSpeed = 0;//ひとつ前の速度 + float oldAccel = 0;//ひとつ前の加速度 + float difference=0;//変位 + float timespan=0.1;//時間差 + float accel[3];//accelを3つの配列で定義。 while(1){ - float accel[3];//accelを3つの配列で定義。 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 - float x = accel[0]/16384;//x軸方向の加速度 - float y = accel[1]/16384;//y軸方向の加速度 + float x = accel[0]/16384-0.043;//x軸方向の加速度 + float y = accel[1]/16384-0.012;//y軸方向の加速度 float z = accel[2]/16384;//z軸方向の加速度 printf("accel x:%f y:%f z:%f\r\n",x,y,z);//加速度の表示 + // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 + (1 - 係数) * センサの値) + lowpassValue = lowpassValue * filterCoefficient + x * (1 - filterCoefficient); + // ハイパスフィルター(センサの値 - ローパスフィルターの値) + highpassValue = x - lowpassValue; + + // 速度計算(加速度を台形積分する) + speed = ((highpassValue + oldAccel) * timespan) / 2 + speed; + oldAccel = highpassValue; + + // 変位計算(速度を台形積分する) + difference = ((speed + oldSpeed) * timespan) / 2 + difference; + oldSpeed = speed; + + printf(" speed %f difference %f\n",speed,difference); wait(0.1); } }