Diff: imu.cpp

Revision:

76:7fd3ac1afe3e

Parent:

74:f67062e7813e

Child:

87:89bbbcdb667b

--- a/imu.cpp	Tue Jul 20 11:57:05 2021 +0000
+++ b/imu.cpp	Sat Aug 07 06:54:58 2021 +0000
@@ -3,13 +3,13 @@
 void getIMUval()
 {
     // gx gy gz ax ay az
-    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-    ax = ax - agoffset[0];
-    ay = ay - agoffset[1];
-    az = az - agoffset[2];
-    gx = gx - agoffset[3];
-    gy = gy - agoffset[4];
-    gz = gz - agoffset[5];
+    //accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+    ax = accelgyro.getAccelerationX() - agoffset[0];
+    ay = accelgyro.getAccelerationY() - agoffset[1];
+    az = accelgyro.getAccelerationZ() - agoffset[2];
+    gx = accelgyro.getRotationX() - agoffset[3];
+    gy = accelgyro.getRotationY() - agoffset[4];
+    gz = accelgyro.getRotationZ() - agoffset[5];
     // 加速度値を分解能で割って加速度(G)に変換する
     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);
@@ -21,6 +21,14 @@
     gyro.z = float(gz) / GYRO_SSF * 0.0174533f;
     gyro = gyroMedian.Process(gyro);
     mag_sensor.getAxis(mdata); // flush the magnetmeter
+    float magval[3] = {0.0f, 0.0f, 0.0f};
+    magval[0] = (mdata.x - magbias[0]);
+    magval[1] = (mdata.y - magbias[1]);
+    magval[2] = (mdata.z - magbias[2]);
+    mag.x = -magval[1]/magbias[3];
+    mag.y = -magval[0]/magbias[3];
+    mag.z =  magval[2]/magbias[3];
+    /*
     float inputMag[3];
     float outputMag[3];
     inputMag[0] = mdata.x;
@@ -30,5 +38,6 @@
     mag.x = -outputMag[1];
     mag.y = -outputMag[0];
     mag.z =  outputMag[2];
+    */
     mag = magMedian.Process(mag);
 }   
\ No newline at end of file