Diff: Imu.cpp

Revision:

72:1c8d13de08c9

Parent:

71:b821c4c990b7

Child:

73:0720e5550476

--- a/Imu.cpp	Mon Mar 11 10:55:06 2019 +0000
+++ b/Imu.cpp	Mon Mar 11 11:48:58 2019 +0000
@@ -39,13 +39,19 @@
     
     extern Serial sp;
     
-    static float oldYaw=0;
-    float dy = oldYaw - iYaw;//１週前との差
-    if(dy>=180){dy-=360;}
-    if(dy<=-180){dy+=360;}
-    dy = limitYawRate(dy);
-    oldYaw -= dy;//次の週で使う過去データ更新
-    return oldYaw;
+    static float localYaw = 0;
+    float dy = localYaw - (iYaw + yawOfset);//１週前との差
+    if(dy >= 180){
+        dy -= 360;
+        yawOfset += 360;
+    }
+    if(dy <= -180){
+        dy += 360;
+        yawOfset -= 360;
+    }
+    dy = limitYawRate(dy);//変化量に制限を掛ける
+    localYaw -= dy;
+    return localYaw;
 }
 
 //----------------------------------------------------------------
@@ -78,7 +84,7 @@
     //プロパティ初期化
     yaw = yaw_ref = 0.0f;
     gyroZ = gyro_ref = 0;
-    yawOfset=0;
+    yawOfset = 0;
     yawRateLimit = YAW_RATE_LIMIT * UPDATE_RATE;//センサ実測値をベースにリミット値決定
 }