Diff: main.cpp

Revision:

12:33085c983354

Parent:

11:5a744949ba5a

Child:

13:b444c425e194

--- a/main.cpp	Fri Jan 11 01:34:11 2013 +0000
+++ b/main.cpp	Fri Jan 11 04:12:00 2013 +0000
@@ -36,7 +36,7 @@
     
     int flag_lc =   0;
     int flag_cm =   0;
-    int loop    =   100;       // for 2 minuts -> loop = 1200
+    int loop    =   1200;       // for 2 minuts -> loop = 1200
     
     char  buf_char;
     unsigned int    buf_hex_rx[30]      =   {0};
@@ -67,7 +67,7 @@
     // ******************** TORATANI ********************
     
     // ******************** HOSHINO *********************
-     float   time    =   0;      //time
+    float   time    =   0;      //time
     float   xis     =   0;
     float   ets     =   0;
     float   psis    =   0;
@@ -109,8 +109,8 @@
     
     //homotopy
     float rps0 = 90.0;
-    float rxi0 = -600.0;    //terminal xi
-    float ret0 = -200.0;    //terminal et
+    float rxi0 = -60.0;    //terminal xi
+    float ret0 = -20.0;    //terminal et
     float ps0  = rps0*dtr;
     float xi0  = rxi0/drf;
     float et0  = ret0/drf;
@@ -192,7 +192,7 @@
     // // flag_lc==1 is debug mode. skip this while loop. //
     // ////////// for flight mode, flag_lc is 0! ///////////
     // /////////////////////////////////////////////////////
-    while(flag_lc==1){
+    while(flag_lc==0){
         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];  }
@@ -1099,8 +1099,8 @@
             ps0  =   azi;
             ht0  =   alt;
             }
-            ets  =   (GPS[0]  -   x0)*cos(ps0);
-            xis  =   (GPS[1]  -   y0)*sin(ps0);
+            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);
             psr  =   pi/2   -(azi     -   ps0);
             htr  =   50+(alt-ht0);
             avp  =   gyro[0];
@@ -1156,7 +1156,7 @@
             avpc=1/tt1*(gkd*pse+gkp*ete);//
             */
             
-            //avpc=kp*hte;
+            avpc=kp*hte;
             
             //vertical
             /*
@@ -1168,17 +1168,19 @@
             //avqc=(-1/tt2*(2/(0.084*rho*v2*swg))*(wgt*grv*cos(gmr*dtr)-wgt*(2*zet2*omg2*dhdt+omg2*omg2*hte)))*pi/180;
             //hte00=hte;
             
-            //avqc=kq*(pse+ete);
+            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;
             
+            
             // transrating dec to hex
             for(int i=0;i<3;i++){  
                 buf_dec_tx[i]   =   19660.5/pi*com[i]+32768;
@@ -1193,7 +1195,7 @@
             // ******************** control law **********************
                         
             // !!! HOSHINO !!! fprintf add guidance, velocity and control
-            fprintf(fp, "%6.1f %7.3f %7.3f %7.3f %8.5f %8.1f %8.1f %8.1f\r\n",time, gyro[0], gyro[1], gyro[2], azi, alt, GPS[0], GPS[1]);
+            fprintf(fp, "%6.1f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %8.5f %8.5f %8.1f %8.1f %8.1f %8.1f %8.1f %8.1f\r\n",time ,gyro[0],avpc, gyro[1],avqc, gyro[2],avrc, psr,psc, htr,htc, xir,xic,etr,etc);
                         
             time = time + 0.1;
             flag_cm++;