Diff: Imu.cpp

Revision:

58:fb7876549148

Parent:

25:f3a6e7eec9c3

Child:

62:4b873328d3a8

--- a/Imu.cpp	Thu Mar 07 04:01:10 2019 +0000
+++ b/Imu.cpp	Thu Mar 07 10:06:34 2019 +0000
@@ -40,12 +40,25 @@
     
     static float oldYaw=0;
     float dy = oldYaw - iYaw;//１週前との差
+    dy = limitYawRate(dy);
     if(dy>=180){yawOfset+=360;}
     if(dy<=-180){yawOfset-=360;}
-    oldYaw = iYaw;//次の週で使う過去データ更新
+    oldYaw -= dy;//次の週で使う過去データ更新
     return iYaw + yawOfset;
 }
 
+//----------------------------------------------------------------
+// Yaw角の変化量に制限をかける 値飛び対策
+//----------------------------------------------------------------
+float Imu::limitYawRate(float yawDiff){
+    float tmpDiff = yawDiff;
+    if (yawRateLimit < fabs(yawDiff * UPDATE_RATE)){
+        const float limitedDiff = yawRateLimit / static_cast<float>(UPDATE_RATE); 
+        tmpDiff = 0 < yawDiff ? limitedDiff : -limitedDiff;
+    }
+    return tmpDiff;
+}
+
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // UART 受信割り込みハンドラ for Arduino UNO(Six Axis Sensor)
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
@@ -65,6 +78,7 @@
     yaw = yaw_ref = 0.0f;
     gyroZ = gyro_ref = 0;
     yawOfset=0;
+    yawRateLimit = 100.0 * UPDATE_RATE;//センサ実測値をベースにリミット値決定
 }
 
 //======================================================