Diff: main.cpp

Revision:

1:bbbebd8be115

Parent:

0:eae101ae93c0

--- a/main.cpp	Wed Feb 06 10:54:28 2019 +0000
+++ b/main.cpp	Fri Feb 08 06:54:54 2019 +0000
@@ -18,6 +18,8 @@
 
 Timer t;
 
+bool setupFlag=false;
+
 
 
 int main() {
@@ -62,18 +64,21 @@
     FirstPITCH = nowAngle[PITCH];
     nowAngle[ROLL] -=FirstROLL;
     nowAngle[PITCH] -=FirstPITCH;
+    FirstYAW = nowAngle[YAW];
+    nowAngle[YAW] -= FirstYAW;
     
     wait(0.2);
     
     
     pc.printf("All initialized\r\n");
+    setupFlag=true;
 }
 
 void SensingMPU(){
     //static int16_t deltaT = 0, t_start = 0;
     //t_start = t.read_us();
     
-    float rpy[3] = {0}, oldrpy[3] = {0};
+    float rpy[3] = {0};
     static uint16_t count_changeRPY = 0;
     static bool flg_checkoutlier = false;
     NVIC_DisableIRQ(USART1_IRQn);
@@ -95,7 +100,16 @@
     for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/PI;
     rpy[ROLL] -= FirstROLL;
     rpy[PITCH] -= FirstPITCH;
-    rpy[YAW] -= FirstYAW;
+    if(!setupFlag){
+        rpy[YAW] -= FirstYAW;
+        }else{
+        if(rpy[YAW] >= FirstYAW){
+            rpy[YAW] -= FirstYAW;
+            }else{
+            rpy[YAW] += 360.0f;
+            rpy[YAW] -= FirstYAW;
+        }
+    }
     
     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){