imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test

skipper_raspi_uart_test

Dependencies:   mbed MPU6050_2 HMC5883L_4 SDFileSystem3

Diff: main.cpp

Revision:
34:0c2b67b31b12
Parent:
33:5f77118eacd5
Child:
35:0f89eff001a7
diff -r 5f77118eacd5 -r 0c2b67b31b12 main.cpp
--- a/main.cpp	Fri Mar 01 08:54:00 2019 +0000
+++ b/main.cpp	Fri Mar 01 11:03:39 2019 +0000
@@ -748,7 +748,11 @@
     for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
     if(!flg_checkoutlier || count_changeRPY >= 2){
         for(uint8_t i=0; i<3; i++){
-            nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f;  //2つの移動平均
+            if(rpy[i] > nowAngle[i]-180 && rpy[i] < nowAngle[i]+180){
+                nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f;  //2つの移動平均
+            }else{
+                nowAngle[i] = rpy[i];
+            }
         }
         count_changeRPY = 0;
     }else   count_changeRPY++;