Diff: main.cpp

Revision:

7:05a718fdef74

Parent:

6:3792b68b1fc9

Child:

8:de86cf9ccb89

--- a/main.cpp	Wed Jan 09 08:08:53 2013 +0000
+++ b/main.cpp	Thu Jan 10 05:59:12 2013 +0000
@@ -128,9 +128,41 @@
     float   et[mt]={0.0};
     float   xi1[mt]={0.0};
     float   et1[mt]={0.0};
+    float   psc;
+    float   etc;
     //********************* HOSHINO *********************
+ 
+    //********************* OHTSU ***********************
+    //teigi
+    float   grv=9.80665;
+    float   swg=0.04695;
+    float   wgt=0.76;
+    float   rho=1.22547328574415;
+    
     
-    //********************* OHTSU ***********************
+    float   tt1=0.8;
+    float   omg1=1.0;
+    float   zet1=0.7;   
+    float   tt2=0.37;
+    float   omg2=2.6;
+    float   zet2=0.7;
+    
+    float   gmr=20;
+    float   vmr=0;
+    float   pse;
+    float   ete;   
+    float   hte;
+    
+    float   hte00;
+    float   dhdt;
+
+    float   v2;
+    
+    float   gkp;
+    float   gkd;
+
+    float   avpc;
+    float   avqc;
     
     //********************* OHTSU ***********************
     
@@ -1095,6 +1127,24 @@
             // ******************** control law **********************
             // transrating dec to hex
             //com
+            
+            pse=(psc-psr)*dtr;
+            ete=etc-etr;
+            hte=htc-htr;
+            
+            //horizontal
+            gkp=omg1*omg1/grv;
+            gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr;
+            avpc=1/tt1*(gkd*pse+gkp*ete);//
+            
+            //vertical
+            v2=vmr*vmr;
+            dhdt=(hte-hte00)/dt;
+           
+            avqc=-1/tt2*(2/(0.084*rho*v2*swg))*(wgt*grv*cos(gmr*dtr)-wgt*(2*zet2*omg2*dhdt+omg2*omg2*hte));//
+            hte00=hte;
+            
+            
             // ******************** control law **********************
             
             // !!! HOSHINO !!! fprintf add guidance, velocity and control