Diff: imu.cpp

Revision:

70:99f974d8960e

Parent:

68:b9f6938fab9d

Child:

74:f67062e7813e

--- a/imu.cpp	Mon Jun 28 01:45:12 2021 +0000
+++ b/imu.cpp	Tue Jun 29 08:07:55 2021 +0000
@@ -11,13 +11,15 @@
     gy = gy - agoffset[4];
     gz = gz - agoffset[5];
     // 加速度値を分解能で割って加速度(G)に変換する
-    acc.x = float(ax) / ACCEL_SSF;  //FS_SEL_0 16,384 LSB / g
-    acc.y = float(ay) / ACCEL_SSF;
-    acc.z = float(az) / ACCEL_SSF;
+    acc.x = mapfloat(float(ax) / ACCEL_SSF,accMin[0],accMax[0],-1.0f,1.0f);  //FS_SEL_0 16,384 LSB / g
+    acc.y = mapfloat(float(ay) / ACCEL_SSF,accMin[1],accMax[1],-1.0f,1.0f);
+    acc.z = mapfloat(float(az) / ACCEL_SSF,accMin[2],accMax[2],-1.0f,1.0f);
+    acc = accMedian.Process(acc);
     // 角速度値を分解能で割って角速度(rad per sec)に変換する
     gyro.x = float(gx) / GYRO_SSF * 0.0174533f;  // (rad/s)
     gyro.y = float(gy) / GYRO_SSF * 0.0174533f;
     gyro.z = float(gz) / GYRO_SSF * 0.0174533f;
+    gyro = gyroMedian.Process(gyro);
     mag_sensor.getAxis(mdata); // flush the magnetmeter
     float inputMag[3];
     float outputMag[3];
@@ -28,4 +30,5 @@
     mag.x = -outputMag[0];
     mag.y = -outputMag[1];
     mag.z = -outputMag[2];
+    mag = magMedian.Process(mag);
 }   
\ No newline at end of file