Diff: main.cpp

Revision:

2:b88f73d7073c

Parent:

1:fc5988ebfa53

Child:

3:8c01b4f33389

--- a/main.cpp	Wed Aug 17 06:40:34 2016 +0000
+++ b/main.cpp	Wed Aug 17 12:47:59 2016 +0000
@@ -280,7 +280,7 @@
     // Set Datalenght & Frame
     sbus_.format(8, Serial::Even, 2);
     //start sbus irq
-    //sbus_.attach(&catch_sbus, RawSerial::RxIrq);
+    sbus_.attach(&catch_sbus, RawSerial::RxIrq);
     }
 
 //---CheckSW_up---
@@ -342,11 +342,11 @@
     mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
     mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
 
-    //  Auto_Loop()で50ms待ってるから要らないと思う
+    /*  Auto_Loop()で50ms待ってるから要らないと思う
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - count;
     if (delt_t > 500) { // update LCD once per half-second independent of read rate
-    
+    */
 
     //pc.printf("ax = %f", 1000*ax); 
     //pc.printf(" ay = %f", 1000*ay); 
@@ -387,13 +387,13 @@
     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
     roll  *= 180.0f / PI;
 
-    pc.printf("Yaw: %f  Pitch:%f  Roll:%f\n\r",  yaw,   pitch,   roll);
+    //pc.printf("Yaw: %f  Pitch:%f  Roll:%f\n\r",  yaw,   pitch,   roll);
     //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
  
     count = t.read_ms(); 
     sum = 0;
     sumCount = 0; 
-    }
+    //}           
     
 }
     
@@ -402,14 +402,14 @@
 void StabiGyro()
 {
     if(jj<=25){     //旋回し始め
-        Auto_RUD = RUD_N - 30 - int((roll+130)*0.75);
-        Auto_ELE = ELE_N + 30+ int((pitch-30)*0.8);
+        Auto_RUD = RUD_N - 30 - int((gy+130)*0.75);   //roll -> gy
+        Auto_ELE = ELE_N + 30 - int((gx-30)*0.8);     //pitch  -> -gx
         Auto_THR = THR_N + 50;     
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else{          //旋回中
-        Auto_RUD = RUD_N - 20 - int((roll+25)*0.75);
-        Auto_ELE = ELE_N - 10 + int((pitch-30)*0.8);
+        Auto_RUD = RUD_N - 20 - int((gy+25)*0.75); 
+        Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
         Auto_THR = THR_N;
     }
     
@@ -434,30 +434,30 @@
 void StabiGyro_Mobius()
 {
     if(jj<=25){     //右旋回導入
-        Auto_RUD = RUD_N - 30 - int((roll+130)*0.75);
-        Auto_ELE = ELE_N + 30+ int((pitch-30)*0.8);
+        Auto_RUD = RUD_N - 30 - int((gy+130)*0.75);
+        Auto_ELE = ELE_N + 30 - int((gx-30)*0.8);
         Auto_THR = THR_N + 50;     
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else if((25<jj) && (jj<=112)){     //右旋回途中
-        Auto_RUD = RUD_N - 20 - int((roll+25)*0.75);
-        Auto_ELE = ELE_N - 10 + int((pitch-30)*0.8);
+        Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
+        Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
         Auto_THR = THR_N;
         jj++;
     }else if((112<jj) && (jj<=117)){    //旋回遷移1
-        Auto_RUD = RUD_N + 160 - int((roll-100)*0.75);
-        Auto_ELE = ELE_N + 20 + int((pitch+0)*0.6);
+        Auto_RUD = RUD_N + 160 - int((gy-100)*0.75);
+        Auto_ELE = ELE_N + 20 - int((gx+0)*0.6);
         Auto_THR = THR_N - 100;     
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else if((117<jj) && (jj<=132)){    //旋回遷移2
-        Auto_RUD = RUD_N + 120 - int((roll-100)*0.75);
-        Auto_ELE = ELE_N + 65 + int((pitch-30)*0.6);
+        Auto_RUD = RUD_N + 120 - int((gy-100)*0.75);
+        Auto_ELE = ELE_N + 65 - int((gx-30)*0.6);
         Auto_THR = THR_N - 20;
         jj++;
     }else{                              //左旋回途中
-        Auto_RUD = RUD_N + 120 - int((roll-25)*0.75);
-        Auto_ELE = ELE_N + 0 + int((pitch-30)*0.6);
+        Auto_RUD = RUD_N + 120 - int((gy-25)*0.75);
+        Auto_ELE = ELE_N + 0 - int((gx-30)*0.6);
         Auto_THR = THR_N + 30;
     }
         
@@ -481,14 +481,14 @@
 void StabiGyro_Climb()
 {
     if(jj<=25){     //右旋回導入
-        Auto_RUD = RUD_N - 30 - int((roll+130)*0.75);
-        Auto_ELE = ELE_N + 30+ int((pitch-30)*0.8);
+        Auto_RUD = RUD_N - 30 - int((gy+130)*0.75);
+        Auto_ELE = ELE_N + 30 - int((gx-30)*0.8);
         Auto_THR = THR_N + 50;     
         //pc.printf("first%d\r\n",jj);
         jj++;
     }else if((25<jj) && (jj<=224)){     //右旋回途中
-        Auto_RUD = RUD_N - 20 - int((roll+25)*0.75);
-        Auto_ELE = ELE_N - 10 + int((pitch-30)*0.8);
+        Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
+        Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
         Auto_THR = THR_N;
         jj++;
     /*}else if((224<jj) && (jj<=234)){    //上昇遷移
@@ -498,13 +498,13 @@
         //pc.printf("first%d\r\n",jj);
         jj++;*/
     }else if((224<jj) && (jj<=314)){    //上昇中
-        Auto_RUD = RUD_N - 10 - int((roll+25)*0.75);
-        Auto_ELE = ELE_N - 35 + int((pitch-30)*0.8);
+        Auto_RUD = RUD_N - 10 - int((gy+25)*0.75);
+        Auto_ELE = ELE_N - 35 - int((gx-30)*0.8);
         Auto_THR = THR_N + 220;
         jj++;
     }else{                              //水平旋回
-        Auto_RUD = RUD_N - 20 - int((roll+25)*0.75);
-        Auto_ELE = ELE_N - 10 + int((pitch-30)*0.8);
+        Auto_RUD = RUD_N - 20 - int((gy+25)*0.75);
+        Auto_ELE = ELE_N - 10 - int((gx-30)*0.8);
         Auto_THR = THR_N;
     }
         
@@ -534,8 +534,8 @@
         //pc.printf("first%d\r\n",jj);
     //    jj++;
     //}else{
-        Auto_RUD = RUD_N - 10 - int((roll+10)*0.75);
-        Auto_ELE = ELE_N - 10 + int((pitch-15)*0.8);
+        Auto_RUD = RUD_N - 10 - int((gy+10)*0.75);
+        Auto_ELE = ELE_N - 10 - int((gx-15)*0.8);
         Auto_THR = 1000;    //スロットルoff
     //}
     
@@ -683,7 +683,7 @@
         //pc.printf("ch%d=%d ", i+1, channels[i]);
         //}
         //pc.printf("\n");
-        wait_ms(500); 
+        //wait_ms(500); 
         
         
         switch(OperationMode){      //変数OperationModeの値で場合分け