yal kaiyo
/
YNU_MPU_tora
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
YNU_MPU_1
by 
yal kaiyo
Revision 14:feea7e2a51b8, committed 2013-01-14
- Comitter:
- yal_kaiyo
- Date:
- Mon Jan 14 05:48:31 2013 +0000
- Parent:
- 13:b444c425e194
- Child:
- 15:571b34671309
- Commit message:
- add safety mode, time deference
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Jan 13 09:13:17 2013 +0000
+++ b/main.cpp Mon Jan 14 05:48:31 2013 +0000
@@ -1,8 +1,8 @@
#include "mbed.h"
#include "SDFileSystem.h"
-#define pi 3.1416
-#define data 6
+#define pi 3.1416
+#define data 6
#define zer 0.0
#define hal 0.5
@@ -200,7 +200,7 @@
// // flag_lc==1 is debug mode. skip this while loop. //
// ////////// for flight mode, flag_lc is 0! ///////////
// /////////////////////////////////////////////////////
- while(flag_lc==0){
+ while(flag_lc==1){
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]; }
@@ -1112,8 +1112,8 @@
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);
+ xis = (GPS[0] - x0)*cos(pi/2-ps0)+(GPS[1] - y0)*sin(pi/2-ps0);
+ ets = -(GPS[0] - x0)*cos(pi/2-ps0)+(GPS[1] - y0)*sin(pi/2-ps0);
psr = rps0*dtr -(azi - ps0);
htr = alt;
avp = gyro[0];
@@ -1122,20 +1122,25 @@
// ******************** initialization *******************
// ******************** interpolation ********************
- drp = drp + ua * dt;
- xir = xir + ua * dt * cos(psr);
- etr = etr + ua * dt * sin(psr);
+ //xir ni hennkannseneba
+
+ //filter henko
if (xis != xis1 && 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);
drp= drc;
- xir=xis;
- etr=ets;
+ xir=xis + ua * dt * cos(psr);
+ etr=ets + ua * dt * sin(psr);
xis1=xis;
ets1=ets;
}
+ else{
+ drp = drp + ua * dt;
+ xir = xir + ua * dt * cos(psr);
+ etr = etr + ua * dt * sin(psr);
+ }
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
@@ -1144,7 +1149,7 @@
b0 = ht0
b1 = (hf-ht0)/drf
*/
-
+ /*
htc = b0 + b1*drp + b2*drp*drp + b3*drp*drp*drp;
for(int i=0;i<mt;i++){
@@ -1160,7 +1165,10 @@
break;
}
}
-
+ */
+ htc = ht0;
+ xic = 0;
+ etc = 0;
// ******************** guidance law WO homo *************
// ******************** control law **********************
@@ -1189,16 +1197,17 @@
//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;
+ if(hte > 12 || xic > 12 || etc >12){
+ com[0] = 0;
+ com[1] = 0;
+ com[2] = 0;
+ }
// transrating dec to hex
for(int i=0;i<3;i++){