Diff: main.cpp

Revision:

13:b444c425e194

Parent:

12:33085c983354

Child:

14:feea7e2a51b8

--- a/main.cpp	Fri Jan 11 04:12:00 2013 +0000
+++ b/main.cpp	Sun Jan 13 09:13:17 2013 +0000
@@ -36,7 +36,7 @@
     
     int flag_lc =   0;
     int flag_cm =   0;
-    int loop    =   1200;       // for 2 minuts -> loop = 1200
+    int loop    =   100;       // for 2 minuts -> loop = 1200
     
     char  buf_char;
     unsigned int    buf_hex_rx[30]      =   {0};
@@ -68,49 +68,59 @@
     
     // ******************** HOSHINO *********************
     float   time    =   0;      //time
+    float   flag_int=   0;
+    //initial condition
+    float   drf     =   100;    //syuutandaunrenji
+    float   ht0     =   0;
+    float   gm0     =   20*pi/180;
+    float   ua      =   10; //initial horizontal velosity
+    //terminal condition
+    float   htf     =   5;    
+    float   gmf     =   5*pi/180;
+     
     float   xis     =   0;
     float   ets     =   0;
     float   psis    =   0;
     float   x0      =   0;
-    float   y0      =   0;
-    float   drf     =   100;    //syuutandaunrenji
-    float   ht0     =   50;
-    float   htf     =   5;
-    float   gm0     =   20*pi/180;
-    float   gmf     =   5*pi/180;
-    float   b0      =   ht0;
-    float   b1      =   tan(gm0);
-    float   b2      =   (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf); 
-    float   b3      =   -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
+    float   y0      =   0; 
+    float   h0      =   0;    
+    float   b0      =   0;
+    float   b1      =   0;
+    float   b2      =   0;
+    float   b3      =   0;
     float   htc     =   0;
     float   x       =   0;  //down range
-    float   ua      =   10;    //initial horizontal velosity
     float   xis1    =   0;
     float   ets1    =   0;
     float   xir     =   0;
     float   etr     =   0;
     float   drc     =   0;
-    float   drp      =   0;
+    float   drp     =   0;
     float   dt      =   0.1;
     float   psr     =   0;
     float   htr     =   0;
-    float   avp = 0;
-    float   avq = 0;
-    float   avr = 0;
-    float   s = 0;
-    float   c=0;
-    float   c0=0;
-    float   c1=0;
-    float   dr[mt]={0};
-    float   time1=0;
-    float   kp=0;
-    float   kq=0;
-    float   kr=0;
+    float   avp     =   0;
+    float   avq     =   0;
+    float   avr     =   0;
+    float   s       =   0;
+    float   c       =   0;
+    float   c0      =   0;
+    float   c1      =   0;
+    float   dr[mt]  =   {0};
+    float   time1   =   0;
+    float   kp      =   0;
+    float   kq      =   0;
+    float   kr      =   0;
+    float   psc     =   0;
+    float   etc     =   0;
+    float   xic     =   0;
     
     //homotopy
+    //initial condition
     float rps0 = 90.0;
     float rxi0 = -60.0;    //terminal xi
     float ret0 = -20.0;    //terminal et
+    
     float ps0  = rps0*dtr;
     float xi0  = rxi0/drf;
     float et0  = ret0/drf;
@@ -131,20 +141,18 @@
     float fpi[mh][mh] = {0};
     float x1[4][mt]   = {0};
     float rl[4][mt]   = {0};
-    float qmax=0.0;
-    float rdet =1.0;
-    float idet =0.0;
+    float qmax=0;
+    float rdet =1;
+    float idet =0;
     float t = 0.0;
     float phf[mh]={0};
-    float   ap[mt]={0.0};
-    float   ps[mt]={0.0};
-    float   xi[mt]={0.0};
-    float   et[mt]={0.0};
-    float   xi1[mt]={0.0};
-    float   et1[mt]={0.0};
-    float   psc;
-    float   etc;
-    float   xic;
+    float   ap[mt]={0};
+    float   ps[mt]={0};
+    float   xi[mt]={0};
+    float   et[mt]={0};
+    float   xi1[mt]={0};
+    float   et1[mt]={0};
+
     //********************* HOSHINO *********************
  
     //********************* OHTSU ***********************
@@ -1093,16 +1101,21 @@
             for(int i=0;i<mt;i++){
             dr[i]=i*drf/md;
             }
-            if(t == 0){
+            if(flag_int == 0){
             x0   =   GPS[0];
             y0   =   GPS[1];
             ps0  =   azi;
             ht0  =   alt;
+            b0      =   ht0;
+            b1      =   tan(gm0);
+            b2      =   (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf); 
+            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);
-            psr  =   pi/2   -(azi     -   ps0);
-            htr  =   50+(alt-ht0);
+            psr  =   rps0*dtr   -(azi     -   ps0);
+            htr  =   alt;
             avp  =   gyro[0];
             avq  =   gyro[1];
             avr  =   gyro[2];
@@ -1126,6 +1139,12 @@
             // ******************** interpolation ********************
             
             // ******************** guidance law WO homo *************
+
+            /*
+            b0      =   ht0
+            b1      =   (hf-ht0)/drf
+            */
+            
             htc =    b0  +   b1*drp    +   b2*drp*drp  +   b3*drp*drp*drp;
             
             for(int i=0;i<mt;i++){
@@ -1156,7 +1175,7 @@
             avpc=1/tt1*(gkd*pse+gkp*ete);//
             */
             
-            avpc=kp*hte;
+            //avpc=kp*hte;
             
             //vertical
             /*
@@ -1168,14 +1187,14 @@
             //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;