Diff: main.cpp

Revision:

14:feea7e2a51b8

Parent:

13:b444c425e194

Child:

15:571b34671309

--- a/main.cpp	Sun Jan 13 09:13:17 2013 +0000
+++ b/main.cpp	Mon Jan 14 05:48:31 2013 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
 #include "SDFileSystem.h"
 
-#define pi      3.1416
-#define data    6
+#define pi   3.1416
+#define data 6
 
 #define zer 0.0
 #define hal 0.5
@@ -200,7 +200,7 @@
     // // flag_lc==1 is debug mode. skip this while loop. //
     // ////////// for flight mode, flag_lc is 0! ///////////
     // /////////////////////////////////////////////////////
-    while(flag_lc==0){
+    while(flag_lc==1){
         buf_char       =   mavc.getc();
         buf_hex_rx[0]  =   (unsigned int)buf_char;
         for(int i=0;i<29;i++){  buf_hex_rx[29-i]   =   buf_hex_rx[28-i];  }
@@ -1112,8 +1112,8 @@
             b3      =   -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
             flag_int=1;            
             }
-            xis  =   (GPS[0]  -   x0)*cos(pi/2-ps0)-(GPS[1] - y0)*sin(pi/2-ps0);
-            ets  =   (GPS[1]  -   y0)*sin(pi/2-ps0)+(GPS[0] - x0)*cos(pi/2-ps0);
+            xis  =   (GPS[0]  -   x0)*cos(pi/2-ps0)+(GPS[1] - y0)*sin(pi/2-ps0);
+            ets  =   -(GPS[0] - x0)*cos(pi/2-ps0)+(GPS[1]  -   y0)*sin(pi/2-ps0);
             psr  =   rps0*dtr   -(azi     -   ps0);
             htr  =   alt;
             avp  =   gyro[0];
@@ -1122,20 +1122,25 @@
             // ******************** initialization *******************
             
             // ******************** interpolation ********************
-            drp = drp + ua * dt;
-            xir = xir + ua * dt * cos(psr);
-            etr = etr + ua * dt * sin(psr);
+            //xir ni hennkannseneba
+
+            //filter henko
 
             if (xis != xis1 && fabs(xis-xis1)<50) {
                 s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1);
                 ua = 0.5*(ua+sqrt(s)/(time-time1));
                 drc = drc+ua*(time-time1);
                 drp= drc;
-                xir=xis;
-                etr=ets;
+                xir=xis + ua * dt * cos(psr);
+                etr=ets + ua * dt * sin(psr);
                 xis1=xis;
                 ets1=ets;
             }
+            else{
+                drp = drp + ua * dt;
+                xir = xir + ua * dt * cos(psr);
+                etr = etr + ua * dt * sin(psr);
+            }
             // ******************** interpolation ********************
             
             // ******************** guidance law WO homo *************
@@ -1144,7 +1149,7 @@
             b0      =   ht0
             b1      =   (hf-ht0)/drf
             */
-            
+            /*
             htc =    b0  +   b1*drp    +   b2*drp*drp  +   b3*drp*drp*drp;
             
             for(int i=0;i<mt;i++){
@@ -1160,7 +1165,10 @@
             break;
              }
             }
-            
+            */
+            htc =   ht0;
+            xic =   0;
+            etc =   0;
             // ******************** guidance law WO homo *************
             
             // ******************** control law **********************
@@ -1189,16 +1197,17 @@
             
             //avqc=kq*(pse+ete);
             
-            // dummy output
-            
-            avpc    =   avpc+0.001*pi;
-            avqc    =   0;
-            avrc    =   avrc-0.001*pi;
+
             
             com[0]  =   avpc;
             com[1]  =   avqc;
             com[2]  =   avrc;
             
+            if(hte > 12 || xic > 12 || etc >12){
+            com[0]  =   0;
+            com[1]  =   0;
+            com[2]  =   0;
+            }
             
             // transrating dec to hex
             for(int i=0;i<3;i++){