yal kaiyo / Mbed 2 deprecated 130114_YNU_MPU_1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of 130111_YNU_MPU_4 by yal kaiyo

Files at this revision

API Documentation at this revision

Revision 16:a33fe7d99a0f, committed 2013-01-16

Comitter:
yal_kaiyo
Date:
Wed Jan 16 13:04:38 2013 +0000
Parent:
15:571b34671309
Commit message:
a

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/main.cpp	Mon Jan 14 12:02:15 2013 +0000
+++ b/main.cpp	Wed Jan 16 13:04:38 2013 +0000
@@ -73,7 +73,7 @@
     float   drf     =   100;    //syuutandaunrenji
     float   ht0     =   0;
     float   gm0     =   20*pi/180;
-    float   ua      =   10; //initial horizontal velosity
+    float   ua      =   1; //initial horizontal velosity
     //terminal condition
     float   htf     =   5;    
     float   gmf     =   5*pi/180;
@@ -118,7 +118,7 @@
     
     //homotopy
     //initial condition
-    float rps0 = 90.0;
+    float rps0 =  0.0;
     float rxi0 = -60.0;    //terminal xi
     float ret0 = -20.0;    //terminal et
     
@@ -171,7 +171,7 @@
     float   zet2    =   0.7;
     
     float   gmr     =   20;
-    float   vmr     =   0;
+    float   vmr     =   16;
     float   pse     =   0;
     float   phir    =   0;
     float   ete     =   0;   
@@ -1135,11 +1135,8 @@
             // ******************** initialization *******************
             
             // ******************** interpolation ********************
-            //xir ni hennkannseneba
 
-            //filter henko
-
-            if (xis != xis1 && fabs(xis-xis1)<50) {
+            if (fabs(xis- xis1)>1 && 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);
@@ -1172,6 +1169,7 @@
             if(c >=0 && c1 >= 0){
             c0=c/(dr[i+1]-dr[i]);
             c1=c1/(dr[i+1]-dr[i]);
+            apc=c0*ap[i+1]+c1*ap[i];
             psc=c0*ps[i+1]+c1*ps[i];
             xic=c0*xi[i+1]+c1*xi[i];
             etc=c0*et[i+1]+c1*et[i];
@@ -1198,10 +1196,7 @@
             phff=atan(apc*v2/grv);
             phic=-gkp*ete-gkd*pse-phff;
             avpc=(phic-phic00)/dt;
-            phic00=phic;
-            
-            
-            
+            phic00=phic;                      
             //vertical
             
             a22=grv*sin(gmr*dtr)/vmr;
@@ -1210,21 +1205,14 @@
             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;
-            
-            
-            
+            ck2=(a23*a32+omg2*omg2)/aaa;         
             
             dhdt=(hte-hte00)/dt;
-            
-            
+                   
             // to translate deg/s to rad/s, add *pi/180. TORATANI 
             avqc=(ck1*dhdt+ck2*hte)/tt2;
             hte00=hte;
-            
-            
-
-            
+                       
             com[0]  =   avpc;
             com[1]  =   avqc;
             com[2]  =   avrc;