Diff: main.cpp

Revision:

15:571b34671309

Parent:

14:feea7e2a51b8

Child:

16:a33fe7d99a0f

--- a/main.cpp	Mon Jan 14 05:48:31 2013 +0000
+++ b/main.cpp	Mon Jan 14 12:02:15 2013 +0000
@@ -114,6 +114,7 @@
     float   psc     =   0;
     float   etc     =   0;
     float   xic     =   0;
+    float   apc     =   0;
     
     //homotopy
     //initial condition
@@ -172,17 +173,29 @@
     float   gmr     =   20;
     float   vmr     =   0;
     float   pse     =   0;
+    float   phir    =   0;
     float   ete     =   0;   
     float   hte     =   0;
+    float   xie     =   0;
     
     float   hte00   =   0;
-    float   dhdt    =   0;
+    float   dhdt    =   0; 
+    float   phic00  =   0;
+    float   dclda   =   0.084;
 
     float   v2      =   0;
+    float   a22     =   0;
+    float   a23     =   0;
+    float   a32     =   0;
+    float   b21     =   0;
+    float   aaa     =   0;
     
     float   gkp     =   0;
     float   gkd     =   0;
-
+    float   ck1     =   0;
+    float   ck2     =   0;
+    float   phff    =   0;
+    float   phic    =   0;
     float   avpc    =   0;
     float   avqc    =   0;
     float   avrc    =   0;
@@ -1172,30 +1185,43 @@
             // ******************** guidance law WO homo *************
             
             // ******************** control law **********************
+            xie=xic-xir;
             pse=(psc-psr)*dtr;
             ete=etc-etr;
             hte=htc-htr;
             
             //horizontal
-            /*
+             v2=vmr*vmr;
+            
             gkp=omg1*omg1/grv;
             gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr;
-            avpc=1/tt1*(gkd*pse+gkp*ete);//
-            */
+            phff=atan(apc*v2/grv);
+            phic=-gkp*ete-gkd*pse-phff;
+            avpc=(phic-phic00)/dt;
+            phic00=phic;
             
-            //avpc=kp*hte;
+            
             
             //vertical
-            /*
-            v2=vmr*vmr;
-            dhdt=(hte-hte00)/dt
-            */
+            
+            a22=grv*sin(gmr*dtr)/vmr;
+            a23=0;
+            a32=vmr*cos(gmr*dtr);
+            b21=swg*rho*vmr*dclda*cos(phir*dtr)/(2*wgt);
+            aaa=a32*b21;
+            ck1=(a22+2*zet2*omg2)/aaa;
+            ck2=(a23*a32+omg2*omg2)/aaa;
+            
+            
+            
+            
+            dhdt=(hte-hte00)/dt;
+            
             
             // to translate deg/s to rad/s, add *pi/180. TORATANI 
-            //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=(ck1*dhdt+ck2*hte)/tt2;
+            hte00=hte;
             
-            //avqc=kq*(pse+ete);
             
 
             
@@ -1203,7 +1229,7 @@
             com[1]  =   avqc;
             com[2]  =   avrc;
             
-            if(hte > 12 || xic > 12 || etc >12){
+            if(fabs(hte) > 12 || fabs(xie) > 12 || fabs(ete) >12){
             com[0]  =   0;
             com[1]  =   0;
             com[2]  =   0;