yal kaiyo / Mbed 2 deprecated 130111_YNU_MPU_3

kanazawa demonstration

Fork of 130111_YNU_MPU_1 by yal kaiyo

main.cpp@15:571b34671309, 2013-01-14 (annotated)

Committer:
yal_kaiyo
Date:
Mon Jan 14 12:02:15 2013 +0000
Revision:
15:571b34671309
Parent:
14:feea7e2a51b8 
add control law

Who changed what in which revision?

UserRevisionLine numberNew contents of line
higedura 0:9aa9be24636a 1 #include "mbed.h"
higedura 0:9aa9be24636a 2 #include "SDFileSystem.h"
higedura 0:9aa9be24636a 3
yal_kaiyo 14:feea7e2a51b8 4 #define pi 3.1416
yal_kaiyo 14:feea7e2a51b8 5 #define data 6
higedura 0:9aa9be24636a 6
khoshino 2:30f96c159d9c 7 #define zer 0.0
khoshino 2:30f96c159d9c 8 #define hal 0.5
khoshino 2:30f96c159d9c 9 #define one 1.0
khoshino 2:30f96c159d9c 10 #define dtr 0.01745
khoshino 2:30f96c159d9c 11 #define rtd 57.3
khoshino 2:30f96c159d9c 12 //parameter//
khoshino 2:30f96c159d9c 13 #define dt1 0.2 // 1/N
khoshino 2:30f96c159d9c 14 #define hdt 0.1 // 1/2N
khoshino 2:30f96c159d9c 15 #define md 6 //kizami N
khoshino 2:30f96c159d9c 16 #define mt 7 //md+1
khoshino 2:30f96c159d9c 17 #define mx 4
khoshino 2:30f96c159d9c 18 #define mv 8
khoshino 2:30f96c159d9c 19 #define mh 56 //mt*mv
khoshino 2:30f96c159d9c 20
higedura 0:9aa9be24636a 21 Serial pc(USBTX, USBRX);
higedura 0:9aa9be24636a 22 DigitalOut led1(LED1);
higedura 0:9aa9be24636a 23 DigitalOut led2(LED2);
higedura 0:9aa9be24636a 24 DigitalOut led3(LED3);
higedura 0:9aa9be24636a 25 DigitalOut led4(LED4);
higedura 0:9aa9be24636a 26
higedura 0:9aa9be24636a 27 SDFileSystem sd(p5, p6, p7, p8, "sd");
higedura 0:9aa9be24636a 28 Serial mavc(p9, p10);
higedura 0:9aa9be24636a 29 DigitalIn stop_sd(p11);
higedura 0:9aa9be24636a 30
higedura 0:9aa9be24636a 31 int main() {
higedura 1:ca296cfa603b 32
higedura 1:ca296cfa603b 33 // ******************** TORATANI ********************
yal_kaiyo 9:a56c340f47c9 34 pc.baud(921600);
higedura 0:9aa9be24636a 35 mavc.baud(115200);
higedura 0:9aa9be24636a 36
higedura 5:192835ac6106 37 int flag_lc = 0;
higedura 5:192835ac6106 38 int flag_cm = 0;
yal_kaiyo 13:b444c425e194 39 int loop = 100; // for 2 minuts -> loop = 1200
khoshino 4:037fab837823 40
higedura 0:9aa9be24636a 41 char buf_char;
higedura 8:de86cf9ccb89 42 unsigned int buf_hex_rx[30] = {0};
higedura 8:de86cf9ccb89 43 unsigned int buf_dec_rx[10] = {0};
higedura 8:de86cf9ccb89 44 unsigned int buf_dec_tx[3] = {0};
higedura 8:de86cf9ccb89 45 unsigned int buf_hex_tx[3][4] = {0};
higedura 0:9aa9be24636a 46
higedura 0:9aa9be24636a 47 // rx
higedura 0:9aa9be24636a 48 double acc[3] = {0};
higedura 0:9aa9be24636a 49 double gyro[3] = {0};
higedura 0:9aa9be24636a 50 double azi;
higedura 0:9aa9be24636a 51 double alt;
higedura 0:9aa9be24636a 52 double GPS[2] = {0};
higedura 0:9aa9be24636a 53
higedura 0:9aa9be24636a 54 // tx
yal_kaiyo 9:a56c340f47c9 55 float com[3] = {0};
yal_kaiyo 9:a56c340f47c9 56 unsigned int H;
yal_kaiyo 9:a56c340f47c9 57 unsigned int D[data] = {0};
higedura 0:9aa9be24636a 58 H = 0x02; // header of dateset2
yal_kaiyo 9:a56c340f47c9 59 D[0] = 0x7f; D[1] = 0xff; // p_com
yal_kaiyo 9:a56c340f47c9 60 D[2] = 0x7f; D[3] = 0xff; // q_com
yal_kaiyo 9:a56c340f47c9 61 D[4] = 0x7f; D[5] = 0xff; // r_com
yal_kaiyo 9:a56c340f47c9 62 /*
higedura 8:de86cf9ccb89 63 D[0] = 0x7; D[1] = 0xf; D[2] = 0xf; D[3] = 0xf; // p_com
higedura 8:de86cf9ccb89 64 D[4] = 0x7; D[5] = 0xf; D[6] = 0xf; D[7] = 0xf; // q_com
higedura 8:de86cf9ccb89 65 D[8] = 0x7; D[9] = 0xf; D[10] = 0xf; D[11] = 0xf; // r_com
yal_kaiyo 9:a56c340f47c9 66 */
higedura 1:ca296cfa603b 67 // ******************** TORATANI ********************
higedura 0:9aa9be24636a 68
khoshino 2:30f96c159d9c 69 // ******************** HOSHINO *********************
yal_kaiyo 12:33085c983354 70 float time = 0; //time
yal_kaiyo 13:b444c425e194 71 float flag_int= 0;
yal_kaiyo 13:b444c425e194 72 //initial condition
yal_kaiyo 13:b444c425e194 73 float drf = 100; //syuutandaunrenji
yal_kaiyo 13:b444c425e194 74 float ht0 = 0;
yal_kaiyo 13:b444c425e194 75 float gm0 = 20*pi/180;
yal_kaiyo 13:b444c425e194 76 float ua = 10; //initial horizontal velosity
yal_kaiyo 13:b444c425e194 77 //terminal condition
yal_kaiyo 13:b444c425e194 78 float htf = 5;
yal_kaiyo 13:b444c425e194 79 float gmf = 5*pi/180;
yal_kaiyo 13:b444c425e194 80
khoshino 4:037fab837823 81 float xis = 0;
yal_kaiyo 9:a56c340f47c9 82 float ets = 0;
khoshino 2:30f96c159d9c 83 float psis = 0;
khoshino 2:30f96c159d9c 84 float x0 = 0;
yal_kaiyo 13:b444c425e194 85 float y0 = 0;
yal_kaiyo 13:b444c425e194 86 float h0 = 0;
yal_kaiyo 13:b444c425e194 87 float b0 = 0;
yal_kaiyo 13:b444c425e194 88 float b1 = 0;
yal_kaiyo 13:b444c425e194 89 float b2 = 0;
yal_kaiyo 13:b444c425e194 90 float b3 = 0;
yal_kaiyo 9:a56c340f47c9 91 float htc = 0;
khoshino 4:037fab837823 92 float x = 0; //down range
yal_kaiyo 9:a56c340f47c9 93 float xis1 = 0;
yal_kaiyo 9:a56c340f47c9 94 float ets1 = 0;
yal_kaiyo 9:a56c340f47c9 95 float xir = 0;
yal_kaiyo 9:a56c340f47c9 96 float etr = 0;
yal_kaiyo 9:a56c340f47c9 97 float drc = 0;
yal_kaiyo 13:b444c425e194 98 float drp = 0;
yal_kaiyo 9:a56c340f47c9 99 float dt = 0.1;
yal_kaiyo 9:a56c340f47c9 100 float psr = 0;
yal_kaiyo 9:a56c340f47c9 101 float htr = 0;
yal_kaiyo 13:b444c425e194 102 float avp = 0;
yal_kaiyo 13:b444c425e194 103 float avq = 0;
yal_kaiyo 13:b444c425e194 104 float avr = 0;
yal_kaiyo 13:b444c425e194 105 float s = 0;
yal_kaiyo 13:b444c425e194 106 float c = 0;
yal_kaiyo 13:b444c425e194 107 float c0 = 0;
yal_kaiyo 13:b444c425e194 108 float c1 = 0;
yal_kaiyo 13:b444c425e194 109 float dr[mt] = {0};
yal_kaiyo 13:b444c425e194 110 float time1 = 0;
yal_kaiyo 13:b444c425e194 111 float kp = 0;
yal_kaiyo 13:b444c425e194 112 float kq = 0;
yal_kaiyo 13:b444c425e194 113 float kr = 0;
yal_kaiyo 13:b444c425e194 114 float psc = 0;
yal_kaiyo 13:b444c425e194 115 float etc = 0;
yal_kaiyo 13:b444c425e194 116 float xic = 0;
yal_kaiyo 15:571b34671309 117 float apc = 0;
khoshino 2:30f96c159d9c 118
khoshino 2:30f96c159d9c 119 //homotopy
yal_kaiyo 13:b444c425e194 120 //initial condition
khoshino 2:30f96c159d9c 121 float rps0 = 90.0;
yal_kaiyo 12:33085c983354 122 float rxi0 = -60.0; //terminal xi
yal_kaiyo 12:33085c983354 123 float ret0 = -20.0; //terminal et
yal_kaiyo 13:b444c425e194 124
khoshino 2:30f96c159d9c 125 float ps0 = rps0*dtr;
khoshino 2:30f96c159d9c 126 float xi0 = rxi0/drf;
khoshino 2:30f96c159d9c 127 float et0 = ret0/drf;
khoshino 2:30f96c159d9c 128 float eps = 0.00000001;
khoshino 2:30f96c159d9c 129 float ueps = 0.000001;
khoshino 2:30f96c159d9c 130 float ier = 0.0;
khoshino 2:30f96c159d9c 131 float sum = 0.0;
khoshino 4:037fab837823 132 float ph0[mh] = {0};
khoshino 4:037fab837823 133 float phi[mh] = {0};
khoshino 4:037fab837823 134 float fn[mh] = {0};
khoshino 4:037fab837823 135 float wk[mh] = {0};
khoshino 2:30f96c159d9c 136 int iwk[mh] = {0};
khoshino 4:037fab837823 137 float dph1[mh] = {0};
khoshino 4:037fab837823 138 float dph2[mh] = {0};
khoshino 4:037fab837823 139 float dph3[mh] = {0};
khoshino 4:037fab837823 140 float dph4[mh] = {0};
khoshino 4:037fab837823 141 float fph[mh][mh] = {0};
khoshino 4:037fab837823 142 float fpi[mh][mh] = {0};
khoshino 4:037fab837823 143 float x1[4][mt] = {0};
khoshino 4:037fab837823 144 float rl[4][mt] = {0};
yal_kaiyo 13:b444c425e194 145 float qmax=0;
yal_kaiyo 13:b444c425e194 146 float rdet =1;
yal_kaiyo 13:b444c425e194 147 float idet =0;
khoshino 2:30f96c159d9c 148 float t = 0.0;
khoshino 4:037fab837823 149 float phf[mh]={0};
yal_kaiyo 13:b444c425e194 150 float ap[mt]={0};
yal_kaiyo 13:b444c425e194 151 float ps[mt]={0};
yal_kaiyo 13:b444c425e194 152 float xi[mt]={0};
yal_kaiyo 13:b444c425e194 153 float et[mt]={0};
yal_kaiyo 13:b444c425e194 154 float xi1[mt]={0};
yal_kaiyo 13:b444c425e194 155 float et1[mt]={0};
yal_kaiyo 13:b444c425e194 156
khoshino 2:30f96c159d9c 157 //********************* HOSHINO *********************
kenkenpapurika 7:05a718fdef74 158
kenkenpapurika 7:05a718fdef74 159 //********************* OHTSU ***********************
kenkenpapurika 7:05a718fdef74 160 //teigi
yal_kaiyo 9:a56c340f47c9 161 float grv = 9.807;
higedura 8:de86cf9ccb89 162 float swg = 0.04695;
higedura 8:de86cf9ccb89 163 float wgt = 0.76;
yal_kaiyo 9:a56c340f47c9 164 float rho = 1.2255;
khoshino 2:30f96c159d9c 165
higedura 8:de86cf9ccb89 166 float tt1 = 0.8;
higedura 8:de86cf9ccb89 167 float omg1 = 1.0;
higedura 8:de86cf9ccb89 168 float zet1 = 0.7;
higedura 8:de86cf9ccb89 169 float tt2 = 0.37;
higedura 8:de86cf9ccb89 170 float omg2 = 2.6;
higedura 8:de86cf9ccb89 171 float zet2 = 0.7;
kenkenpapurika 7:05a718fdef74 172
higedura 8:de86cf9ccb89 173 float gmr = 20;
higedura 8:de86cf9ccb89 174 float vmr = 0;
higedura 8:de86cf9ccb89 175 float pse = 0;
yal_kaiyo 15:571b34671309 176 float phir = 0;
higedura 8:de86cf9ccb89 177 float ete = 0;
higedura 8:de86cf9ccb89 178 float hte = 0;
yal_kaiyo 15:571b34671309 179 float xie = 0;
kenkenpapurika 7:05a718fdef74 180
higedura 8:de86cf9ccb89 181 float hte00 = 0;
yal_kaiyo 15:571b34671309 182 float dhdt = 0;
yal_kaiyo 15:571b34671309 183 float phic00 = 0;
yal_kaiyo 15:571b34671309 184 float dclda = 0.084;
kenkenpapurika 7:05a718fdef74 185
higedura 8:de86cf9ccb89 186 float v2 = 0;
yal_kaiyo 15:571b34671309 187 float a22 = 0;
yal_kaiyo 15:571b34671309 188 float a23 = 0;
yal_kaiyo 15:571b34671309 189 float a32 = 0;
yal_kaiyo 15:571b34671309 190 float b21 = 0;
yal_kaiyo 15:571b34671309 191 float aaa = 0;
kenkenpapurika 7:05a718fdef74 192
higedura 8:de86cf9ccb89 193 float gkp = 0;
higedura 8:de86cf9ccb89 194 float gkd = 0;
yal_kaiyo 15:571b34671309 195 float ck1 = 0;
yal_kaiyo 15:571b34671309 196 float ck2 = 0;
yal_kaiyo 15:571b34671309 197 float phff = 0;
yal_kaiyo 15:571b34671309 198 float phic = 0;
higedura 8:de86cf9ccb89 199 float avpc = 0;
higedura 8:de86cf9ccb89 200 float avqc = 0;
higedura 8:de86cf9ccb89 201 float avrc = 0;
khoshino 4:037fab837823 202 //********************* OHTSU ***********************
khoshino 4:037fab837823 203
higedura 0:9aa9be24636a 204 FILE *fp = fopen("/sd/sdtest.txt", "w");
higedura 0:9aa9be24636a 205 if(fp == NULL) {
higedura 0:9aa9be24636a 206 error("Could not open file for write\n");
higedura 0:9aa9be24636a 207 }
higedura 0:9aa9be24636a 208
yal_kaiyo 9:a56c340f47c9 209 // **************** waiting @LC or @FB ****************
yal_kaiyo 9:a56c340f47c9 210 // * @LC:0x40 0x4C 0x43 0x0D, @FB:0x40 0x46 0x42 0x0D *
higedura 8:de86cf9ccb89 211 // /////////////////////////////////////////////////////
higedura 8:de86cf9ccb89 212 // ////////////////////// NOTICE! //////////////////////
higedura 8:de86cf9ccb89 213 // // flag_lc==1 is debug mode. skip this while loop. //
higedura 8:de86cf9ccb89 214 // ////////// for flight mode, flag_lc is 0! ///////////
higedura 8:de86cf9ccb89 215 // /////////////////////////////////////////////////////
yal_kaiyo 14:feea7e2a51b8 216 while(flag_lc==1){
higedura 5:192835ac6106 217 buf_char = mavc.getc();
higedura 8:de86cf9ccb89 218 buf_hex_rx[0] = (unsigned int)buf_char;
higedura 8:de86cf9ccb89 219 for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; }
yal_kaiyo 9:a56c340f47c9 220 if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x46 && buf_hex_rx[27]==0x42 && buf_hex_rx[26]==0x0D){
higedura 5:192835ac6106 221 // ******************** waiting @LC ********************
higedura 5:192835ac6106 222
higedura 5:192835ac6106 223 // ******************** homotopy ********************
higedura 5:192835ac6106 224
khoshino 6:3792b68b1fc9 225 ////SYOKIKATEIKAI////
khoshino 6:3792b68b1fc9 226 for(int it=0;it<mt;it++){
khoshino 6:3792b68b1fc9 227 double tm=dt1*it;
khoshino 6:3792b68b1fc9 228 int i0=mv*it;
khoshino 6:3792b68b1fc9 229 ph0[i0]=zer;
khoshino 6:3792b68b1fc9 230 if(fabs(ps0)<=0.0000000001){
khoshino 6:3792b68b1fc9 231 ph0[i0+1]=(ps0+1.0)*tm;
khoshino 6:3792b68b1fc9 232 }
khoshino 6:3792b68b1fc9 233 else{
khoshino 6:3792b68b1fc9 234 ph0[i0+1] = ps0*tm;
khoshino 6:3792b68b1fc9 235 }
khoshino 6:3792b68b1fc9 236 ph0[i0+2]= xi0*tm;
khoshino 6:3792b68b1fc9 237 ph0[i0+3]= et0*tm;
khoshino 6:3792b68b1fc9 238 ph0[i0+4]=-hal;
khoshino 6:3792b68b1fc9 239 ph0[i0+5]=-hal;
khoshino 6:3792b68b1fc9 240 ph0[i0+6]=-hal;
khoshino 6:3792b68b1fc9 241 ph0[i0+7]=-hal;
khoshino 6:3792b68b1fc9 242 }
khoshino 6:3792b68b1fc9 243 for(int it=0;it<mh;it++){
khoshino 6:3792b68b1fc9 244 phi[it]=ph0[it];
khoshino 6:3792b68b1fc9 245 }
khoshino 6:3792b68b1fc9 246 for(int it=0;it<mh;it++){
khoshino 6:3792b68b1fc9 247 phf[it]=ph0[it];
khoshino 6:3792b68b1fc9 248 }
khoshino 6:3792b68b1fc9 249
khoshino 6:3792b68b1fc9 250 //rungekutta
khoshino 6:3792b68b1fc9 251 for(int ir=0;ir<mt;ir++){
khoshino 6:3792b68b1fc9 252 //k1
khoshino 6:3792b68b1fc9 253
khoshino 6:3792b68b1fc9 254 ////SABUNSHIKI//////
khoshino 6:3792b68b1fc9 255 for(int it=0;it<mt;it++){
khoshino 6:3792b68b1fc9 256 int i0=mv*it;
khoshino 6:3792b68b1fc9 257 x1[0][it]=phi[i0+0];
khoshino 6:3792b68b1fc9 258 x1[1][it]=phi[i0+1];
khoshino 6:3792b68b1fc9 259 x1[2][it]=phi[i0+2];
khoshino 6:3792b68b1fc9 260 x1[3][it]=phi[i0+3];
khoshino 6:3792b68b1fc9 261 rl[0][it]=phi[i0+4];
khoshino 6:3792b68b1fc9 262 rl[1][it]=phi[i0+5];
khoshino 6:3792b68b1fc9 263 rl[2][it]=phi[i0+6];
khoshino 6:3792b68b1fc9 264 rl[3][it]=phi[i0+7];
khoshino 6:3792b68b1fc9 265 }
khoshino 6:3792b68b1fc9 266
khoshino 6:3792b68b1fc9 267 fn[0]=x1[0][0];
khoshino 6:3792b68b1fc9 268 fn[1]=x1[1][0];
khoshino 6:3792b68b1fc9 269 fn[2]=x1[2][0];
khoshino 6:3792b68b1fc9 270 fn[3]=x1[3][0];
khoshino 6:3792b68b1fc9 271 for(int it=0;it<md;it++){
khoshino 6:3792b68b1fc9 272 int i0=mx+mv*it;
khoshino 6:3792b68b1fc9 273 int it1=it+1;
khoshino 6:3792b68b1fc9 274 double bkp = hal*(x1[0][it]+x1[0][it1]);
khoshino 6:3792b68b1fc9 275 double bps = hal*(x1[1][it]+x1[1][it1]);
khoshino 6:3792b68b1fc9 276 double br1 = hal*(rl[0][it]+rl[0][it1]);
khoshino 6:3792b68b1fc9 277 double br2 = hal*(rl[1][it]+rl[1][it1]);
khoshino 6:3792b68b1fc9 278 double br3 = hal*(rl[2][it]+rl[2][it1]);
khoshino 6:3792b68b1fc9 279 double br4 = hal*(rl[3][it]+rl[3][it1]);
khoshino 6:3792b68b1fc9 280 fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
khoshino 6:3792b68b1fc9 281 fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
khoshino 6:3792b68b1fc9 282 fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
khoshino 6:3792b68b1fc9 283 fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
khoshino 6:3792b68b1fc9 284 fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
khoshino 6:3792b68b1fc9 285 fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
khoshino 6:3792b68b1fc9 286 fn[i0+6] = rl[2][it1]-rl[2][it];
khoshino 6:3792b68b1fc9 287 fn[i0+7] = rl[3][it1]-rl[3][it];
khoshino 6:3792b68b1fc9 288 }
khoshino 6:3792b68b1fc9 289 fn[mh-4]=x1[0][mt-1];
khoshino 6:3792b68b1fc9 290 fn[mh-3]=x1[1][mt-1]-ps0;
khoshino 6:3792b68b1fc9 291 fn[mh-2]=x1[2][mt-1]-xi0;
khoshino 6:3792b68b1fc9 292 fn[mh-1]=x1[3][mt-1]-et0;
khoshino 6:3792b68b1fc9 293
khoshino 6:3792b68b1fc9 294
khoshino 6:3792b68b1fc9 295 ////HENBIBUN////////////
khoshino 6:3792b68b1fc9 296 //// I 0 //////
khoshino 6:3792b68b1fc9 297 //// A B 0 ////
khoshino 6:3792b68b1fc9 298 //// 0 A B 0 //
khoshino 6:3792b68b1fc9 299 ///////////////
khoshino 6:3792b68b1fc9 300 ////////////I 0
khoshino 6:3792b68b1fc9 301 //I
khoshino 6:3792b68b1fc9 302 fph[0][0]=one;
khoshino 6:3792b68b1fc9 303 fph[1][1]=one;
khoshino 6:3792b68b1fc9 304 fph[2][2]=one;
khoshino 6:3792b68b1fc9 305 fph[3][3]=one;
khoshino 6:3792b68b1fc9 306 //A
khoshino 6:3792b68b1fc9 307 for(int iu=0;iu<md;iu++){
khoshino 6:3792b68b1fc9 308 int i0=mx+mv*iu;
khoshino 6:3792b68b1fc9 309 int j0=mv*iu;
khoshino 6:3792b68b1fc9 310 int j1=j0+mv;
khoshino 6:3792b68b1fc9 311 double bps=hal*(phi[j0+1]+phi[j1+1]);
khoshino 6:3792b68b1fc9 312 double br3=hal*(phi[j0+6]+phi[j1+6]);
khoshino 6:3792b68b1fc9 313 double br4=hal*(phi[j0+7]+phi[j1+7]);
khoshino 6:3792b68b1fc9 314 double cps=cos(bps);
khoshino 6:3792b68b1fc9 315 double sps=sin(bps);
khoshino 6:3792b68b1fc9 316 for(int iv=0;iv<mv;iv++){
khoshino 6:3792b68b1fc9 317 fph[i0+iv][j0+iv]=-one;
khoshino 6:3792b68b1fc9 318 }
khoshino 6:3792b68b1fc9 319 fph[i0+1][j0] = hdt;
khoshino 6:3792b68b1fc9 320 fph[i0+2][j0+1] =-hdt*sps;
khoshino 6:3792b68b1fc9 321 fph[i0+3][j0+1] = hdt*cps;
khoshino 6:3792b68b1fc9 322 fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
khoshino 6:3792b68b1fc9 323 fph[i0][j0+4] = hdt;
khoshino 6:3792b68b1fc9 324 fph[i0+4][j0+5] =-hdt;
khoshino 6:3792b68b1fc9 325 fph[i0+5][j0+6] = hdt*sps;
khoshino 6:3792b68b1fc9 326 fph[i0+5][j0+7] =-hdt*cps;
khoshino 6:3792b68b1fc9 327 }
khoshino 6:3792b68b1fc9 328 //B
khoshino 6:3792b68b1fc9 329 //j0=j0+mv;
khoshino 6:3792b68b1fc9 330 for(int iu=0;iu<md;iu++){
khoshino 6:3792b68b1fc9 331 int i0=mx+mv*iu;
khoshino 6:3792b68b1fc9 332 int j0=mv*(iu+1);
khoshino 6:3792b68b1fc9 333 int j1=j0+mv;
khoshino 6:3792b68b1fc9 334 double bps=hal*(phi[j0+1]+phi[j1+1]);
khoshino 6:3792b68b1fc9 335 double br3=hal*(phi[j0+6]+phi[j1+6]);
khoshino 6:3792b68b1fc9 336 double br4=hal*(phi[j0+7]+phi[j1+7]);
khoshino 6:3792b68b1fc9 337 double cps=cos(bps);
khoshino 6:3792b68b1fc9 338 double sps=sin(bps);
khoshino 6:3792b68b1fc9 339 for(int iv=0;iv<mv;iv++){
khoshino 6:3792b68b1fc9 340 fph[i0+iv][j0+iv]=one;
khoshino 6:3792b68b1fc9 341 }
khoshino 6:3792b68b1fc9 342 fph[i0+1][j0]=hdt;
khoshino 6:3792b68b1fc9 343 fph[i0+2][j0+1]=-hdt*sps;
khoshino 6:3792b68b1fc9 344 fph[i0+3][j0+1]=hdt*cps;
khoshino 6:3792b68b1fc9 345 fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
khoshino 6:3792b68b1fc9 346 fph[i0][j0+4]=hdt;
khoshino 6:3792b68b1fc9 347 fph[i0+4][j0+5]=-hdt;
khoshino 6:3792b68b1fc9 348 fph[i0+5][j0+6]=hdt*sps;
khoshino 6:3792b68b1fc9 349 fph[i0+5][j0+7]=-hdt*cps;
khoshino 6:3792b68b1fc9 350 }
khoshino 6:3792b68b1fc9 351 //I
khoshino 6:3792b68b1fc9 352 fph[mh-4][mh-8]=one;
khoshino 6:3792b68b1fc9 353 fph[mh-3][mh-7]=one;
khoshino 6:3792b68b1fc9 354 fph[mh-2][mh-6]=one;
khoshino 6:3792b68b1fc9 355 fph[mh-1][mh-5]=one;
khoshino 6:3792b68b1fc9 356
khoshino 6:3792b68b1fc9 357 //GYAKUGYOURETSU
khoshino 6:3792b68b1fc9 358 for(int i=0;i<mh;i++){
khoshino 6:3792b68b1fc9 359 for(int j=0;j<mh;j++){
khoshino 6:3792b68b1fc9 360 fpi[i][j]=fph[i][j];
khoshino 6:3792b68b1fc9 361 }
khoshino 6:3792b68b1fc9 362 }
khoshino 6:3792b68b1fc9 363 for(int j=0;j<mh;j++) {
khoshino 6:3792b68b1fc9 364 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 365 if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
khoshino 6:3792b68b1fc9 366 }
khoshino 6:3792b68b1fc9 367 }
khoshino 6:3792b68b1fc9 368
khoshino 6:3792b68b1fc9 369 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 370 double q=wk[i];
khoshino 6:3792b68b1fc9 371 // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
khoshino 6:3792b68b1fc9 372 wk[i]=1.0/q;
khoshino 6:3792b68b1fc9 373 if(q > qmax){qmax=q;}
khoshino 6:3792b68b1fc9 374 }
khoshino 6:3792b68b1fc9 375
khoshino 6:3792b68b1fc9 376 if(eps < 0){eps=qmax*mh*ueps;}
khoshino 6:3792b68b1fc9 377 rdet =1.0;
khoshino 6:3792b68b1fc9 378 idet =0.0;
khoshino 6:3792b68b1fc9 379 for(int k=0;k<mh;k++) {
khoshino 6:3792b68b1fc9 380 double amax = 0.0;
khoshino 6:3792b68b1fc9 381 int kmax = k;
khoshino 6:3792b68b1fc9 382 for(int i=k;i<mh;i++) {
khoshino 6:3792b68b1fc9 383 if(fabs(fpi[i][k])*wk[i] <= amax){}
khoshino 6:3792b68b1fc9 384 else{
khoshino 6:3792b68b1fc9 385 amax=fabs(fpi[i][k])*wk[i];
khoshino 6:3792b68b1fc9 386 kmax=i;
khoshino 6:3792b68b1fc9 387 }
khoshino 6:3792b68b1fc9 388 }
khoshino 6:3792b68b1fc9 389 //if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
khoshino 6:3792b68b1fc9 390 rdet=rdet*fpi[kmax][k];
khoshino 6:3792b68b1fc9 391 if(k != kmax){rdet=-rdet;}
khoshino 6:3792b68b1fc9 392 double adet =fabs(rdet);
khoshino 6:3792b68b1fc9 393 while(adet > 1.0){
khoshino 6:3792b68b1fc9 394 adet = adet*0.0625;
khoshino 6:3792b68b1fc9 395 idet = idet + 4;
khoshino 6:3792b68b1fc9 396 }
khoshino 6:3792b68b1fc9 397 if(adet >= 0.0625){}
khoshino 6:3792b68b1fc9 398 else{
khoshino 6:3792b68b1fc9 399 adet=adet*16.0;
khoshino 6:3792b68b1fc9 400 idet=idet-4;
khoshino 6:3792b68b1fc9 401 }
khoshino 6:3792b68b1fc9 402 if(rdet < 0.0){adet=-adet;}
khoshino 6:3792b68b1fc9 403 rdet=adet;
khoshino 6:3792b68b1fc9 404 iwk[k]=kmax;
khoshino 6:3792b68b1fc9 405 amax=-1.0/fpi[kmax][k];
khoshino 6:3792b68b1fc9 406 t=fpi[kmax][k];
khoshino 6:3792b68b1fc9 407 fpi[kmax][k]=fpi[k][k];
khoshino 6:3792b68b1fc9 408 fpi[k][k]=t;
khoshino 6:3792b68b1fc9 409 for(int j=0;j<mh;j++) {
khoshino 6:3792b68b1fc9 410 if(j == k){}
khoshino 6:3792b68b1fc9 411 else{
khoshino 6:3792b68b1fc9 412 t=fpi[kmax][j]*amax;
khoshino 6:3792b68b1fc9 413 fpi[kmax][j]=fpi[k][j];
khoshino 6:3792b68b1fc9 414 if(fabs(t) <= 0){}
khoshino 6:3792b68b1fc9 415 else{
khoshino 6:3792b68b1fc9 416 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 417 fpi[i][j]=fpi[i][j]+t*fpi[i][k];
khoshino 6:3792b68b1fc9 418 }
khoshino 6:3792b68b1fc9 419 }
khoshino 6:3792b68b1fc9 420 fpi[k][j]=-t;
khoshino 6:3792b68b1fc9 421 }
khoshino 6:3792b68b1fc9 422 }
khoshino 6:3792b68b1fc9 423 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 424 fpi[i][k]=fpi[i][k]*amax;
khoshino 6:3792b68b1fc9 425 }
khoshino 6:3792b68b1fc9 426 fpi[k][k]=-amax;
khoshino 6:3792b68b1fc9 427 }
khoshino 6:3792b68b1fc9 428
khoshino 6:3792b68b1fc9 429 for(int l=0;l<mh;l++) {
khoshino 6:3792b68b1fc9 430 int k=mh-l-1;
khoshino 6:3792b68b1fc9 431 if(k == iwk[k]){}
khoshino 6:3792b68b1fc9 432 else{
khoshino 6:3792b68b1fc9 433 int kmax=iwk[k];
khoshino 6:3792b68b1fc9 434 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 435 t=fpi[i][k];
khoshino 6:3792b68b1fc9 436 fpi[i][k]=fpi[i][kmax];
khoshino 6:3792b68b1fc9 437 fpi[i][kmax]=t;
khoshino 6:3792b68b1fc9 438 }
khoshino 6:3792b68b1fc9 439 }
khoshino 6:3792b68b1fc9 440 }
khoshino 6:3792b68b1fc9 441
khoshino 6:3792b68b1fc9 442 //////function
khoshino 6:3792b68b1fc9 443 for(int jh=0;jh<mh;jh++){
khoshino 6:3792b68b1fc9 444 sum=zer;
khoshino 6:3792b68b1fc9 445 for(int ih=0;ih<mh;ih++){
khoshino 6:3792b68b1fc9 446 sum=sum-fpi[jh][ih]*fn[ih];
khoshino 6:3792b68b1fc9 447 }
khoshino 6:3792b68b1fc9 448 dph1[jh]=sum;
khoshino 6:3792b68b1fc9 449 }
khoshino 6:3792b68b1fc9 450
khoshino 6:3792b68b1fc9 451 for(int it=0;it<mh;it++){
khoshino 6:3792b68b1fc9 452 phi[it]= phf[it]+hal*dt1*dph1[it];
khoshino 6:3792b68b1fc9 453 }
khoshino 6:3792b68b1fc9 454
khoshino 6:3792b68b1fc9 455
khoshino 6:3792b68b1fc9 456 //k2
khoshino 6:3792b68b1fc9 457 ////SABUNSHIKI//////
khoshino 6:3792b68b1fc9 458 for(int it=0;it<mt;it++){
khoshino 6:3792b68b1fc9 459 int i0=mv*it;
khoshino 6:3792b68b1fc9 460 x1[0][it]=phi[i0+0];
khoshino 6:3792b68b1fc9 461 x1[1][it]=phi[i0+1];
khoshino 6:3792b68b1fc9 462 x1[2][it]=phi[i0+2];
khoshino 6:3792b68b1fc9 463 x1[3][it]=phi[i0+3];
khoshino 6:3792b68b1fc9 464 rl[0][it]=phi[i0+4];
khoshino 6:3792b68b1fc9 465 rl[1][it]=phi[i0+5];
khoshino 6:3792b68b1fc9 466 rl[2][it]=phi[i0+6];
khoshino 6:3792b68b1fc9 467 rl[3][it]=phi[i0+7];
khoshino 6:3792b68b1fc9 468 }
khoshino 6:3792b68b1fc9 469
khoshino 6:3792b68b1fc9 470 fn[0]=x1[0][0];
khoshino 6:3792b68b1fc9 471 fn[1]=x1[1][0];
khoshino 6:3792b68b1fc9 472 fn[2]=x1[2][0];
khoshino 6:3792b68b1fc9 473 fn[3]=x1[3][0];
khoshino 6:3792b68b1fc9 474 for(int it=0;it<md;it++){
khoshino 6:3792b68b1fc9 475 int i0=mx+mv*it;
khoshino 6:3792b68b1fc9 476 int it1=it+1;
khoshino 6:3792b68b1fc9 477 double bkp = hal*(x1[0][it]+x1[0][it1]);
khoshino 6:3792b68b1fc9 478 double bps = hal*(x1[1][it]+x1[1][it1]);
khoshino 6:3792b68b1fc9 479 double br1 = hal*(rl[0][it]+rl[0][it1]);
khoshino 6:3792b68b1fc9 480 double br2 = hal*(rl[1][it]+rl[1][it1]);
khoshino 6:3792b68b1fc9 481 double br3 = hal*(rl[2][it]+rl[2][it1]);
khoshino 6:3792b68b1fc9 482 double br4 = hal*(rl[3][it]+rl[3][it1]);
khoshino 6:3792b68b1fc9 483 fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
khoshino 6:3792b68b1fc9 484 fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
khoshino 6:3792b68b1fc9 485 fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
khoshino 6:3792b68b1fc9 486 fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
khoshino 6:3792b68b1fc9 487 fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
khoshino 6:3792b68b1fc9 488 fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
khoshino 6:3792b68b1fc9 489 fn[i0+6] = rl[2][it1]-rl[2][it];
khoshino 6:3792b68b1fc9 490 fn[i0+7] = rl[3][it1]-rl[3][it];
khoshino 6:3792b68b1fc9 491 }
khoshino 6:3792b68b1fc9 492 fn[mh-4]=x1[0][mt-1];
khoshino 6:3792b68b1fc9 493 fn[mh-3]=x1[1][mt-1]-ps0;
khoshino 6:3792b68b1fc9 494 fn[mh-2]=x1[2][mt-1]-xi0;
khoshino 6:3792b68b1fc9 495 fn[mh-1]=x1[3][mt-1]-et0;
khoshino 6:3792b68b1fc9 496
khoshino 6:3792b68b1fc9 497
khoshino 6:3792b68b1fc9 498 ////HENBIBUN////////////
khoshino 6:3792b68b1fc9 499 //// I 0 //////
khoshino 6:3792b68b1fc9 500 //// A B 0 ////
khoshino 6:3792b68b1fc9 501 //// 0 A B 0 //
khoshino 6:3792b68b1fc9 502 ///////////////
khoshino 6:3792b68b1fc9 503 ////////////I 0
khoshino 6:3792b68b1fc9 504 //I
khoshino 6:3792b68b1fc9 505 fph[0][0]=one;
khoshino 6:3792b68b1fc9 506 fph[1][1]=one;
khoshino 6:3792b68b1fc9 507 fph[2][2]=one;
khoshino 6:3792b68b1fc9 508 fph[3][3]=one;
khoshino 6:3792b68b1fc9 509 //A
khoshino 6:3792b68b1fc9 510 for(int iu=0;iu<md;iu++){
khoshino 6:3792b68b1fc9 511 int i0=mx+mv*iu;
khoshino 6:3792b68b1fc9 512 int j0=mv*iu;
khoshino 6:3792b68b1fc9 513 int j1=j0+mv;
khoshino 6:3792b68b1fc9 514 double bps=hal*(phi[j0+1]+phi[j1+1]);
khoshino 6:3792b68b1fc9 515 double br3=hal*(phi[j0+6]+phi[j1+6]);
khoshino 6:3792b68b1fc9 516 double br4=hal*(phi[j0+7]+phi[j1+7]);
khoshino 6:3792b68b1fc9 517 double cps=cos(bps);
khoshino 6:3792b68b1fc9 518 double sps=sin(bps);
khoshino 6:3792b68b1fc9 519 for(int iv=0;iv<mv;iv++){
khoshino 6:3792b68b1fc9 520 fph[i0+iv][j0+iv]=-one;
khoshino 6:3792b68b1fc9 521 }
khoshino 6:3792b68b1fc9 522 fph[i0+1][j0] = hdt;
khoshino 6:3792b68b1fc9 523 fph[i0+2][j0+1] =-hdt*sps;
khoshino 6:3792b68b1fc9 524 fph[i0+3][j0+1] = hdt*cps;
khoshino 6:3792b68b1fc9 525 fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
khoshino 6:3792b68b1fc9 526 fph[i0][j0+4] = hdt;
khoshino 6:3792b68b1fc9 527 fph[i0+4][j0+5] =-hdt;
khoshino 6:3792b68b1fc9 528 fph[i0+5][j0+6] = hdt*sps;
khoshino 6:3792b68b1fc9 529 fph[i0+5][j0+7] =-hdt*cps;
khoshino 6:3792b68b1fc9 530 }
khoshino 6:3792b68b1fc9 531 //B
khoshino 6:3792b68b1fc9 532 //j0=j0+mv;
khoshino 6:3792b68b1fc9 533 for(int iu=0;iu<md;iu++){
khoshino 6:3792b68b1fc9 534 int i0=mx+mv*iu;
khoshino 6:3792b68b1fc9 535 int j0=mv*(iu+1);
khoshino 6:3792b68b1fc9 536 int j1=j0+mv;
khoshino 6:3792b68b1fc9 537 double bps=hal*(phi[j0+1]+phi[j1+1]);
khoshino 6:3792b68b1fc9 538 double br3=hal*(phi[j0+6]+phi[j1+6]);
khoshino 6:3792b68b1fc9 539 double br4=hal*(phi[j0+7]+phi[j1+7]);
khoshino 6:3792b68b1fc9 540 double cps=cos(bps);
khoshino 6:3792b68b1fc9 541 double sps=sin(bps);
khoshino 6:3792b68b1fc9 542 for(int iv=0;iv<mv;iv++){
khoshino 6:3792b68b1fc9 543 fph[i0+iv][j0+iv]=one;
khoshino 6:3792b68b1fc9 544 }
khoshino 6:3792b68b1fc9 545 fph[i0+1][j0]=hdt;
khoshino 6:3792b68b1fc9 546 fph[i0+2][j0+1]=-hdt*sps;
khoshino 6:3792b68b1fc9 547 fph[i0+3][j0+1]=hdt*cps;
khoshino 6:3792b68b1fc9 548 fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
khoshino 6:3792b68b1fc9 549 fph[i0][j0+4]=hdt;
khoshino 6:3792b68b1fc9 550 fph[i0+4][j0+5]=-hdt;
khoshino 6:3792b68b1fc9 551 fph[i0+5][j0+6]=hdt*sps;
khoshino 6:3792b68b1fc9 552 fph[i0+5][j0+7]=-hdt*cps;
khoshino 6:3792b68b1fc9 553 }
khoshino 6:3792b68b1fc9 554 //I
khoshino 6:3792b68b1fc9 555 fph[mh-4][mh-8]=one;
khoshino 6:3792b68b1fc9 556 fph[mh-3][mh-7]=one;
khoshino 6:3792b68b1fc9 557 fph[mh-2][mh-6]=one;
khoshino 6:3792b68b1fc9 558 fph[mh-1][mh-5]=one;
khoshino 6:3792b68b1fc9 559
khoshino 6:3792b68b1fc9 560 //GYAKUGYOURETSU
khoshino 6:3792b68b1fc9 561 for(int i=0;i<mh;i++){
khoshino 6:3792b68b1fc9 562 for(int j=0;j<mh;j++){
khoshino 6:3792b68b1fc9 563 fpi[i][j]=fph[i][j];
khoshino 6:3792b68b1fc9 564 }
khoshino 6:3792b68b1fc9 565 }
khoshino 6:3792b68b1fc9 566 for(int j=0;j<mh;j++) {
khoshino 6:3792b68b1fc9 567 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 568 if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
khoshino 6:3792b68b1fc9 569 }
khoshino 6:3792b68b1fc9 570 }
khoshino 6:3792b68b1fc9 571
khoshino 6:3792b68b1fc9 572 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 573 double q=wk[i];
khoshino 6:3792b68b1fc9 574 // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
khoshino 6:3792b68b1fc9 575 wk[i]=1.0/q;
khoshino 6:3792b68b1fc9 576 if(q > qmax){qmax=q;}
khoshino 6:3792b68b1fc9 577 }
khoshino 6:3792b68b1fc9 578
khoshino 6:3792b68b1fc9 579 if(eps < 0){eps=qmax*mh*ueps;}
khoshino 6:3792b68b1fc9 580 rdet =1.0;
khoshino 6:3792b68b1fc9 581 idet =0.0;
khoshino 6:3792b68b1fc9 582 for(int k=0;k<mh;k++) {
khoshino 6:3792b68b1fc9 583 double amax = 0.0;
khoshino 6:3792b68b1fc9 584 int kmax = k;
khoshino 6:3792b68b1fc9 585 for(int i=k;i<mh;i++) {
khoshino 6:3792b68b1fc9 586 if(fabs(fpi[i][k])*wk[i] <= amax){}
khoshino 6:3792b68b1fc9 587 else{
khoshino 6:3792b68b1fc9 588 amax=fabs(fpi[i][k])*wk[i];
khoshino 6:3792b68b1fc9 589 kmax=i;
khoshino 6:3792b68b1fc9 590 }
khoshino 6:3792b68b1fc9 591 }
khoshino 6:3792b68b1fc9 592 // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
khoshino 6:3792b68b1fc9 593 rdet=rdet*fpi[kmax][k];
khoshino 6:3792b68b1fc9 594 if(k != kmax){rdet=-rdet;}
khoshino 6:3792b68b1fc9 595 double adet =fabs(rdet);
khoshino 6:3792b68b1fc9 596 while(adet > 1.0){
khoshino 6:3792b68b1fc9 597 adet = adet*0.0625;
khoshino 6:3792b68b1fc9 598 idet = idet + 4;
khoshino 6:3792b68b1fc9 599 }
khoshino 6:3792b68b1fc9 600 if(adet >= 0.0625){}
khoshino 6:3792b68b1fc9 601 else{
khoshino 6:3792b68b1fc9 602 adet=adet*16.0;
khoshino 6:3792b68b1fc9 603 idet=idet-4;
khoshino 6:3792b68b1fc9 604 }
khoshino 6:3792b68b1fc9 605 if(rdet < 0.0){adet=-adet;}
khoshino 6:3792b68b1fc9 606 rdet=adet;
khoshino 6:3792b68b1fc9 607 iwk[k]=kmax;
khoshino 6:3792b68b1fc9 608 amax=-1.0/fpi[kmax][k];
khoshino 6:3792b68b1fc9 609 t=fpi[kmax][k];
khoshino 6:3792b68b1fc9 610 fpi[kmax][k]=fpi[k][k];
khoshino 6:3792b68b1fc9 611 fpi[k][k]=t;
khoshino 6:3792b68b1fc9 612 for(int j=0;j<mh;j++) {
khoshino 6:3792b68b1fc9 613 if(j == k){}
khoshino 6:3792b68b1fc9 614 else{
khoshino 6:3792b68b1fc9 615 t=fpi[kmax][j]*amax;
khoshino 6:3792b68b1fc9 616 fpi[kmax][j]=fpi[k][j];
khoshino 6:3792b68b1fc9 617 if(fabs(t) <= 0){}
khoshino 6:3792b68b1fc9 618 else{
khoshino 6:3792b68b1fc9 619 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 620 fpi[i][j]=fpi[i][j]+t*fpi[i][k];
khoshino 6:3792b68b1fc9 621 }
khoshino 6:3792b68b1fc9 622 }
khoshino 6:3792b68b1fc9 623 fpi[k][j]=-t;
khoshino 6:3792b68b1fc9 624 }
khoshino 6:3792b68b1fc9 625 }
khoshino 6:3792b68b1fc9 626 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 627 fpi[i][k]=fpi[i][k]*amax;
khoshino 6:3792b68b1fc9 628 }
khoshino 6:3792b68b1fc9 629 fpi[k][k]=-amax;
khoshino 6:3792b68b1fc9 630 }
khoshino 6:3792b68b1fc9 631
khoshino 6:3792b68b1fc9 632 for(int l=0;l<mh;l++) {
khoshino 6:3792b68b1fc9 633 int k=mh-l-1;
khoshino 6:3792b68b1fc9 634 if(k == iwk[k]){}
khoshino 6:3792b68b1fc9 635 else{
khoshino 6:3792b68b1fc9 636 int kmax=iwk[k];
khoshino 6:3792b68b1fc9 637 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 638 t=fpi[i][k];
khoshino 6:3792b68b1fc9 639 fpi[i][k]=fpi[i][kmax];
khoshino 6:3792b68b1fc9 640 fpi[i][kmax]=t;
khoshino 6:3792b68b1fc9 641 }
khoshino 6:3792b68b1fc9 642 }
khoshino 6:3792b68b1fc9 643 }
khoshino 6:3792b68b1fc9 644
khoshino 6:3792b68b1fc9 645 //////function
khoshino 6:3792b68b1fc9 646 for(int jh=0;jh<mh;jh++){
khoshino 6:3792b68b1fc9 647 sum=zer;
khoshino 6:3792b68b1fc9 648 for(int ih=0;ih<mh;ih++){
khoshino 6:3792b68b1fc9 649 sum=sum-fpi[jh][ih]*fn[ih];
khoshino 6:3792b68b1fc9 650 }
khoshino 6:3792b68b1fc9 651 dph2[jh]=sum;
khoshino 6:3792b68b1fc9 652 }
khoshino 6:3792b68b1fc9 653
khoshino 6:3792b68b1fc9 654 //k3
khoshino 6:3792b68b1fc9 655 for(int it=0;it<mh;it++){
khoshino 6:3792b68b1fc9 656 phi[it]= phf[it]+hal*dt1*dph2[it];
khoshino 6:3792b68b1fc9 657 }
khoshino 6:3792b68b1fc9 658 ////SABUNSHIKI//////
khoshino 6:3792b68b1fc9 659 for(int it=0;it<mt;it++){
khoshino 6:3792b68b1fc9 660 int i0=mv*it;
khoshino 6:3792b68b1fc9 661 x1[0][it]=phi[i0+0];
khoshino 6:3792b68b1fc9 662 x1[1][it]=phi[i0+1];
khoshino 6:3792b68b1fc9 663 x1[2][it]=phi[i0+2];
khoshino 6:3792b68b1fc9 664 x1[3][it]=phi[i0+3];
khoshino 6:3792b68b1fc9 665 rl[0][it]=phi[i0+4];
khoshino 6:3792b68b1fc9 666 rl[1][it]=phi[i0+5];
khoshino 6:3792b68b1fc9 667 rl[2][it]=phi[i0+6];
khoshino 6:3792b68b1fc9 668 rl[3][it]=phi[i0+7];
khoshino 6:3792b68b1fc9 669 }
khoshino 6:3792b68b1fc9 670
khoshino 6:3792b68b1fc9 671 fn[0]=x1[0][0];
khoshino 6:3792b68b1fc9 672 fn[1]=x1[1][0];
khoshino 6:3792b68b1fc9 673 fn[2]=x1[2][0];
khoshino 6:3792b68b1fc9 674 fn[3]=x1[3][0];
khoshino 6:3792b68b1fc9 675 for(int it=0;it<md;it++){
khoshino 6:3792b68b1fc9 676 int i0=mx+mv*it;
khoshino 6:3792b68b1fc9 677 int it1=it+1;
khoshino 6:3792b68b1fc9 678 double bkp = hal*(x1[0][it]+x1[0][it1]);
khoshino 6:3792b68b1fc9 679 double bps = hal*(x1[1][it]+x1[1][it1]);
khoshino 6:3792b68b1fc9 680 double br1 = hal*(rl[0][it]+rl[0][it1]);
khoshino 6:3792b68b1fc9 681 double br2 = hal*(rl[1][it]+rl[1][it1]);
khoshino 6:3792b68b1fc9 682 double br3 = hal*(rl[2][it]+rl[2][it1]);
khoshino 6:3792b68b1fc9 683 double br4 = hal*(rl[3][it]+rl[3][it1]);
khoshino 6:3792b68b1fc9 684 fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
khoshino 6:3792b68b1fc9 685 fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
khoshino 6:3792b68b1fc9 686 fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
khoshino 6:3792b68b1fc9 687 fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
khoshino 6:3792b68b1fc9 688 fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
khoshino 6:3792b68b1fc9 689 fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
khoshino 6:3792b68b1fc9 690 fn[i0+6] = rl[2][it1]-rl[2][it];
khoshino 6:3792b68b1fc9 691 fn[i0+7] = rl[3][it1]-rl[3][it];
khoshino 6:3792b68b1fc9 692 }
khoshino 6:3792b68b1fc9 693 fn[mh-4]=x1[0][mt-1];
khoshino 6:3792b68b1fc9 694 fn[mh-3]=x1[1][mt-1]-ps0;
khoshino 6:3792b68b1fc9 695 fn[mh-2]=x1[2][mt-1]-xi0;
khoshino 6:3792b68b1fc9 696 fn[mh-1]=x1[3][mt-1]-et0;
khoshino 6:3792b68b1fc9 697
khoshino 6:3792b68b1fc9 698
khoshino 6:3792b68b1fc9 699 ////HENBIBUN////////////
khoshino 6:3792b68b1fc9 700 //// I 0 //////
khoshino 6:3792b68b1fc9 701 //// A B 0 ////
khoshino 6:3792b68b1fc9 702 //// 0 A B 0 //
khoshino 6:3792b68b1fc9 703 ///////////////
khoshino 6:3792b68b1fc9 704 ////////////I 0
khoshino 6:3792b68b1fc9 705 //I
khoshino 6:3792b68b1fc9 706 fph[0][0]=one;
khoshino 6:3792b68b1fc9 707 fph[1][1]=one;
khoshino 6:3792b68b1fc9 708 fph[2][2]=one;
khoshino 6:3792b68b1fc9 709 fph[3][3]=one;
khoshino 6:3792b68b1fc9 710 //A
khoshino 6:3792b68b1fc9 711 for(int iu=0;iu<md;iu++){
khoshino 6:3792b68b1fc9 712 int i0=mx+mv*iu;
khoshino 6:3792b68b1fc9 713 int j0=mv*iu;
khoshino 6:3792b68b1fc9 714 int j1=j0+mv;
khoshino 6:3792b68b1fc9 715 double bps=hal*(phi[j0+1]+phi[j1+1]);
khoshino 6:3792b68b1fc9 716 double br3=hal*(phi[j0+6]+phi[j1+6]);
khoshino 6:3792b68b1fc9 717 double br4=hal*(phi[j0+7]+phi[j1+7]);
khoshino 6:3792b68b1fc9 718 double cps=cos(bps);
khoshino 6:3792b68b1fc9 719 double sps=sin(bps);
khoshino 6:3792b68b1fc9 720 for(int iv=0;iv<mv;iv++){
khoshino 6:3792b68b1fc9 721 fph[i0+iv][j0+iv]=-one;
khoshino 6:3792b68b1fc9 722 }
khoshino 6:3792b68b1fc9 723 fph[i0+1][j0] = hdt;
khoshino 6:3792b68b1fc9 724 fph[i0+2][j0+1] =-hdt*sps;
khoshino 6:3792b68b1fc9 725 fph[i0+3][j0+1] = hdt*cps;
khoshino 6:3792b68b1fc9 726 fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
khoshino 6:3792b68b1fc9 727 fph[i0][j0+4] = hdt;
khoshino 6:3792b68b1fc9 728 fph[i0+4][j0+5] =-hdt;
khoshino 6:3792b68b1fc9 729 fph[i0+5][j0+6] = hdt*sps;
khoshino 6:3792b68b1fc9 730 fph[i0+5][j0+7] =-hdt*cps;
khoshino 6:3792b68b1fc9 731 }
khoshino 6:3792b68b1fc9 732 //B
khoshino 6:3792b68b1fc9 733 //j0=j0+mv;
khoshino 6:3792b68b1fc9 734 for(int iu=0;iu<md;iu++){
khoshino 6:3792b68b1fc9 735 int i0=mx+mv*iu;
khoshino 6:3792b68b1fc9 736 int j0=mv*(iu+1);
khoshino 6:3792b68b1fc9 737 int j1=j0+mv;
khoshino 6:3792b68b1fc9 738 double bps=hal*(phi[j0+1]+phi[j1+1]);
khoshino 6:3792b68b1fc9 739 double br3=hal*(phi[j0+6]+phi[j1+6]);
khoshino 6:3792b68b1fc9 740 double br4=hal*(phi[j0+7]+phi[j1+7]);
khoshino 6:3792b68b1fc9 741 double cps=cos(bps);
khoshino 6:3792b68b1fc9 742 double sps=sin(bps);
khoshino 6:3792b68b1fc9 743 for(int iv=0;iv<mv;iv++){
khoshino 6:3792b68b1fc9 744 fph[i0+iv][j0+iv]=one;
khoshino 6:3792b68b1fc9 745 }
khoshino 6:3792b68b1fc9 746 fph[i0+1][j0]=hdt;
khoshino 6:3792b68b1fc9 747 fph[i0+2][j0+1]=-hdt*sps;
khoshino 6:3792b68b1fc9 748 fph[i0+3][j0+1]=hdt*cps;
khoshino 6:3792b68b1fc9 749 fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
khoshino 6:3792b68b1fc9 750 fph[i0][j0+4]=hdt;
khoshino 6:3792b68b1fc9 751 fph[i0+4][j0+5]=-hdt;
khoshino 6:3792b68b1fc9 752 fph[i0+5][j0+6]=hdt*sps;
khoshino 6:3792b68b1fc9 753 fph[i0+5][j0+7]=-hdt*cps;
khoshino 6:3792b68b1fc9 754 }
khoshino 6:3792b68b1fc9 755 //I
khoshino 6:3792b68b1fc9 756 fph[mh-4][mh-8]=one;
khoshino 6:3792b68b1fc9 757 fph[mh-3][mh-7]=one;
khoshino 6:3792b68b1fc9 758 fph[mh-2][mh-6]=one;
khoshino 6:3792b68b1fc9 759 fph[mh-1][mh-5]=one;
khoshino 6:3792b68b1fc9 760
khoshino 6:3792b68b1fc9 761 //GYAKUGYOURETSU
khoshino 6:3792b68b1fc9 762 for(int i=0;i<mh;i++){
khoshino 6:3792b68b1fc9 763 for(int j=0;j<mh;j++){
khoshino 6:3792b68b1fc9 764 fpi[i][j]=fph[i][j];
khoshino 6:3792b68b1fc9 765 }
khoshino 6:3792b68b1fc9 766 }
khoshino 6:3792b68b1fc9 767 for(int j=0;j<mh;j++) {
khoshino 6:3792b68b1fc9 768 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 769 if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
khoshino 6:3792b68b1fc9 770 }
khoshino 6:3792b68b1fc9 771 }
khoshino 6:3792b68b1fc9 772
khoshino 6:3792b68b1fc9 773 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 774 double q=wk[i];
khoshino 6:3792b68b1fc9 775 // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
khoshino 6:3792b68b1fc9 776 wk[i]=1.0/q;
khoshino 6:3792b68b1fc9 777 if(q > qmax){qmax=q;}
khoshino 6:3792b68b1fc9 778 }
khoshino 6:3792b68b1fc9 779
khoshino 6:3792b68b1fc9 780 if(eps < 0){eps=qmax*mh*ueps;}
khoshino 6:3792b68b1fc9 781 rdet =1.0;
khoshino 6:3792b68b1fc9 782 idet =0.0;
khoshino 6:3792b68b1fc9 783 for(int k=0;k<mh;k++) {
khoshino 6:3792b68b1fc9 784 double amax = 0.0;
khoshino 6:3792b68b1fc9 785 int kmax = k;
khoshino 6:3792b68b1fc9 786 for(int i=k;i<mh;i++) {
khoshino 6:3792b68b1fc9 787 if(fabs(fpi[i][k])*wk[i] <= amax){}
khoshino 6:3792b68b1fc9 788 else{
khoshino 6:3792b68b1fc9 789 amax=fabs(fpi[i][k])*wk[i];
khoshino 6:3792b68b1fc9 790 kmax=i;
khoshino 6:3792b68b1fc9 791 }
khoshino 6:3792b68b1fc9 792 }
khoshino 6:3792b68b1fc9 793 // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
khoshino 6:3792b68b1fc9 794 rdet=rdet*fpi[kmax][k];
khoshino 6:3792b68b1fc9 795 if(k != kmax){rdet=-rdet;}
khoshino 6:3792b68b1fc9 796 double adet =fabs(rdet);
khoshino 6:3792b68b1fc9 797 while(adet > 1.0){
khoshino 6:3792b68b1fc9 798 adet = adet*0.0625;
khoshino 6:3792b68b1fc9 799 idet = idet + 4;
khoshino 6:3792b68b1fc9 800 }
khoshino 6:3792b68b1fc9 801 if(adet >= 0.0625){}
khoshino 6:3792b68b1fc9 802 else{
khoshino 6:3792b68b1fc9 803 adet=adet*16.0;
khoshino 6:3792b68b1fc9 804 idet=idet-4;
khoshino 6:3792b68b1fc9 805 }
khoshino 6:3792b68b1fc9 806 if(rdet < 0.0){adet=-adet;}
khoshino 6:3792b68b1fc9 807 rdet=adet;
khoshino 6:3792b68b1fc9 808 iwk[k]=kmax;
khoshino 6:3792b68b1fc9 809 amax=-1.0/fpi[kmax][k];
khoshino 6:3792b68b1fc9 810 t=fpi[kmax][k];
khoshino 6:3792b68b1fc9 811 fpi[kmax][k]=fpi[k][k];
khoshino 6:3792b68b1fc9 812 fpi[k][k]=t;
khoshino 6:3792b68b1fc9 813 for(int j=0;j<mh;j++) {
khoshino 6:3792b68b1fc9 814 if(j == k){}
khoshino 6:3792b68b1fc9 815 else{
khoshino 6:3792b68b1fc9 816 t=fpi[kmax][j]*amax;
khoshino 6:3792b68b1fc9 817 fpi[kmax][j]=fpi[k][j];
khoshino 6:3792b68b1fc9 818 if(fabs(t) <= 0){}
khoshino 6:3792b68b1fc9 819 else{
khoshino 6:3792b68b1fc9 820 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 821 fpi[i][j]=fpi[i][j]+t*fpi[i][k];
khoshino 6:3792b68b1fc9 822 }
khoshino 6:3792b68b1fc9 823 }
khoshino 6:3792b68b1fc9 824 fpi[k][j]=-t;
khoshino 6:3792b68b1fc9 825 }
khoshino 6:3792b68b1fc9 826 }
khoshino 6:3792b68b1fc9 827 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 828 fpi[i][k]=fpi[i][k]*amax;
khoshino 6:3792b68b1fc9 829 }
khoshino 6:3792b68b1fc9 830 fpi[k][k]=-amax;
khoshino 6:3792b68b1fc9 831 }
khoshino 6:3792b68b1fc9 832
khoshino 6:3792b68b1fc9 833 for(int l=0;l<mh;l++) {
khoshino 6:3792b68b1fc9 834 int k=mh-l-1;
khoshino 6:3792b68b1fc9 835 if(k == iwk[k]){}
khoshino 6:3792b68b1fc9 836 else{
khoshino 6:3792b68b1fc9 837 int kmax=iwk[k];
khoshino 6:3792b68b1fc9 838 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 839 t=fpi[i][k];
khoshino 6:3792b68b1fc9 840 fpi[i][k]=fpi[i][kmax];
khoshino 6:3792b68b1fc9 841 fpi[i][kmax]=t;
khoshino 6:3792b68b1fc9 842 }
khoshino 6:3792b68b1fc9 843 }
khoshino 6:3792b68b1fc9 844 }
khoshino 6:3792b68b1fc9 845
khoshino 6:3792b68b1fc9 846 //////function
khoshino 6:3792b68b1fc9 847 for(int jh=0;jh<mh;jh++){
khoshino 6:3792b68b1fc9 848 sum=zer;
khoshino 6:3792b68b1fc9 849 for(int ih=0;ih<mh;ih++){
khoshino 6:3792b68b1fc9 850 sum=sum-fpi[jh][ih]*fn[ih];
khoshino 6:3792b68b1fc9 851 }
khoshino 6:3792b68b1fc9 852 dph3[jh]=sum;
khoshino 6:3792b68b1fc9 853 }
khoshino 6:3792b68b1fc9 854
khoshino 6:3792b68b1fc9 855 //k4
khoshino 6:3792b68b1fc9 856
khoshino 6:3792b68b1fc9 857 for(int it=0;it<mh;it++){
khoshino 6:3792b68b1fc9 858 phi[it]= phf[it]+dt1*dph3[it];
khoshino 6:3792b68b1fc9 859 }
khoshino 6:3792b68b1fc9 860 ////SABUNSHIKI//////
khoshino 6:3792b68b1fc9 861 for(int it=0;it<mt;it++){
khoshino 6:3792b68b1fc9 862 int i0=mv*it;
khoshino 6:3792b68b1fc9 863 x1[0][it]=phi[i0+0];
khoshino 6:3792b68b1fc9 864 x1[1][it]=phi[i0+1];
khoshino 6:3792b68b1fc9 865 x1[2][it]=phi[i0+2];
khoshino 6:3792b68b1fc9 866 x1[3][it]=phi[i0+3];
khoshino 6:3792b68b1fc9 867 rl[0][it]=phi[i0+4];
khoshino 6:3792b68b1fc9 868 rl[1][it]=phi[i0+5];
khoshino 6:3792b68b1fc9 869 rl[2][it]=phi[i0+6];
khoshino 6:3792b68b1fc9 870 rl[3][it]=phi[i0+7];
khoshino 6:3792b68b1fc9 871 }
khoshino 6:3792b68b1fc9 872
khoshino 6:3792b68b1fc9 873 fn[0]=x1[0][0];
khoshino 6:3792b68b1fc9 874 fn[1]=x1[1][0];
khoshino 6:3792b68b1fc9 875 fn[2]=x1[2][0];
khoshino 6:3792b68b1fc9 876 fn[3]=x1[3][0];
khoshino 6:3792b68b1fc9 877 for(int it=0;it<md;it++){
khoshino 6:3792b68b1fc9 878 int i0=mx+mv*it;
khoshino 6:3792b68b1fc9 879 int it1=it+1;
khoshino 6:3792b68b1fc9 880 double bkp = hal*(x1[0][it]+x1[0][it1]);
khoshino 6:3792b68b1fc9 881 double bps = hal*(x1[1][it]+x1[1][it1]);
khoshino 6:3792b68b1fc9 882 double br1 = hal*(rl[0][it]+rl[0][it1]);
khoshino 6:3792b68b1fc9 883 double br2 = hal*(rl[1][it]+rl[1][it1]);
khoshino 6:3792b68b1fc9 884 double br3 = hal*(rl[2][it]+rl[2][it1]);
khoshino 6:3792b68b1fc9 885 double br4 = hal*(rl[3][it]+rl[3][it1]);
khoshino 6:3792b68b1fc9 886 fn[i0+0] = x1[0][it1]-x1[0][it]+dt1*br1;
khoshino 6:3792b68b1fc9 887 fn[i0+1] = x1[1][it1]-x1[1][it]+dt1*bkp;
khoshino 6:3792b68b1fc9 888 fn[i0+2] = x1[2][it1]-x1[2][it]+dt1*cos(bps);
khoshino 6:3792b68b1fc9 889 fn[i0+3] = x1[3][it1]-x1[3][it]+dt1*sin(bps);
khoshino 6:3792b68b1fc9 890 fn[i0+4] = rl[0][it1]-rl[0][it]-dt1*br2;
khoshino 6:3792b68b1fc9 891 fn[i0+5] = rl[1][it1]-rl[1][it]+dt1*(br3*sin(bps)-br4*cos(bps));
khoshino 6:3792b68b1fc9 892 fn[i0+6] = rl[2][it1]-rl[2][it];
khoshino 6:3792b68b1fc9 893 fn[i0+7] = rl[3][it1]-rl[3][it];
khoshino 6:3792b68b1fc9 894 }
khoshino 6:3792b68b1fc9 895 fn[mh-4]=x1[0][mt-1];
khoshino 6:3792b68b1fc9 896 fn[mh-3]=x1[1][mt-1]-ps0;
khoshino 6:3792b68b1fc9 897 fn[mh-2]=x1[2][mt-1]-xi0;
khoshino 6:3792b68b1fc9 898 fn[mh-1]=x1[3][mt-1]-et0;
khoshino 6:3792b68b1fc9 899
khoshino 6:3792b68b1fc9 900
khoshino 6:3792b68b1fc9 901 ////HENBIBUN////////////
khoshino 6:3792b68b1fc9 902 //// I 0 //////
khoshino 6:3792b68b1fc9 903 //// A B 0 ////
khoshino 6:3792b68b1fc9 904 //// 0 A B 0 //
khoshino 6:3792b68b1fc9 905 ///////////////
khoshino 6:3792b68b1fc9 906 ////////////I 0
khoshino 6:3792b68b1fc9 907 //I
khoshino 6:3792b68b1fc9 908 fph[0][0]=one;
khoshino 6:3792b68b1fc9 909 fph[1][1]=one;
khoshino 6:3792b68b1fc9 910 fph[2][2]=one;
khoshino 6:3792b68b1fc9 911 fph[3][3]=one;
khoshino 6:3792b68b1fc9 912 //A
khoshino 6:3792b68b1fc9 913 for(int iu=0;iu<md;iu++){
khoshino 6:3792b68b1fc9 914 int i0=mx+mv*iu;
khoshino 6:3792b68b1fc9 915 int j0=mv*iu;
khoshino 6:3792b68b1fc9 916 int j1=j0+mv;
khoshino 6:3792b68b1fc9 917 double bps=hal*(phi[j0+1]+phi[j1+1]);
khoshino 6:3792b68b1fc9 918 double br3=hal*(phi[j0+6]+phi[j1+6]);
khoshino 6:3792b68b1fc9 919 double br4=hal*(phi[j0+7]+phi[j1+7]);
khoshino 6:3792b68b1fc9 920 double cps=cos(bps);
khoshino 6:3792b68b1fc9 921 double sps=sin(bps);
khoshino 6:3792b68b1fc9 922 for(int iv=0;iv<mv;iv++){
khoshino 6:3792b68b1fc9 923 fph[i0+iv][j0+iv]=-one;
khoshino 6:3792b68b1fc9 924 }
khoshino 6:3792b68b1fc9 925 fph[i0+1][j0] = hdt;
khoshino 6:3792b68b1fc9 926 fph[i0+2][j0+1] =-hdt*sps;
khoshino 6:3792b68b1fc9 927 fph[i0+3][j0+1] = hdt*cps;
khoshino 6:3792b68b1fc9 928 fph[i0+5][j0+1] = hdt*(br3*cps+br4*sps);
khoshino 6:3792b68b1fc9 929 fph[i0][j0+4] = hdt;
khoshino 6:3792b68b1fc9 930 fph[i0+4][j0+5] =-hdt;
khoshino 6:3792b68b1fc9 931 fph[i0+5][j0+6] = hdt*sps;
khoshino 6:3792b68b1fc9 932 fph[i0+5][j0+7] =-hdt*cps;
khoshino 6:3792b68b1fc9 933 }
khoshino 6:3792b68b1fc9 934 //B
khoshino 6:3792b68b1fc9 935 //j0=j0+mv;
khoshino 6:3792b68b1fc9 936 for(int iu=0;iu<md;iu++){
khoshino 6:3792b68b1fc9 937 int i0=mx+mv*iu;
khoshino 6:3792b68b1fc9 938 int j0=mv*(iu+1);
khoshino 6:3792b68b1fc9 939 int j1=j0+mv;
khoshino 6:3792b68b1fc9 940 double bps=hal*(phi[j0+1]+phi[j1+1]);
khoshino 6:3792b68b1fc9 941 double br3=hal*(phi[j0+6]+phi[j1+6]);
khoshino 6:3792b68b1fc9 942 double br4=hal*(phi[j0+7]+phi[j1+7]);
khoshino 6:3792b68b1fc9 943 double cps=cos(bps);
khoshino 6:3792b68b1fc9 944 double sps=sin(bps);
khoshino 6:3792b68b1fc9 945 for(int iv=0;iv<mv;iv++){
khoshino 6:3792b68b1fc9 946 fph[i0+iv][j0+iv]=one;
khoshino 6:3792b68b1fc9 947 }
khoshino 6:3792b68b1fc9 948 fph[i0+1][j0]=hdt;
khoshino 6:3792b68b1fc9 949 fph[i0+2][j0+1]=-hdt*sps;
khoshino 6:3792b68b1fc9 950 fph[i0+3][j0+1]=hdt*cps;
khoshino 6:3792b68b1fc9 951 fph[i0+5][j0+1]=hdt*(br3*cps+br4*sps);
khoshino 6:3792b68b1fc9 952 fph[i0][j0+4]=hdt;
khoshino 6:3792b68b1fc9 953 fph[i0+4][j0+5]=-hdt;
khoshino 6:3792b68b1fc9 954 fph[i0+5][j0+6]=hdt*sps;
khoshino 6:3792b68b1fc9 955 fph[i0+5][j0+7]=-hdt*cps;
khoshino 6:3792b68b1fc9 956 }
khoshino 6:3792b68b1fc9 957 //I
khoshino 6:3792b68b1fc9 958 fph[mh-4][mh-8]=one;
khoshino 6:3792b68b1fc9 959 fph[mh-3][mh-7]=one;
khoshino 6:3792b68b1fc9 960 fph[mh-2][mh-6]=one;
khoshino 6:3792b68b1fc9 961 fph[mh-1][mh-5]=one;
khoshino 6:3792b68b1fc9 962
khoshino 6:3792b68b1fc9 963 //GYAKUGYOURETSU
khoshino 6:3792b68b1fc9 964 for(int i=0;i<mh;i++){
khoshino 6:3792b68b1fc9 965 for(int j=0;j<mh;j++){
khoshino 6:3792b68b1fc9 966 fpi[i][j]=fph[i][j];
khoshino 6:3792b68b1fc9 967 }
khoshino 6:3792b68b1fc9 968 }
khoshino 6:3792b68b1fc9 969 for(int j=0;j<mh;j++) {
khoshino 6:3792b68b1fc9 970 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 971 if(fabs(fpi[i][j]) > wk[i]){wk[i]=fabs(fpi[i][j]); }
khoshino 6:3792b68b1fc9 972 }
khoshino 6:3792b68b1fc9 973 }
khoshino 6:3792b68b1fc9 974
khoshino 6:3792b68b1fc9 975 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 976 double q=wk[i];
khoshino 6:3792b68b1fc9 977 // if(q == 0.0){printf("MATRIX IS SINGULAR1\n");}
khoshino 6:3792b68b1fc9 978 wk[i]=1.0/q;
khoshino 6:3792b68b1fc9 979 if(q > qmax){qmax=q;}
khoshino 6:3792b68b1fc9 980 }
khoshino 6:3792b68b1fc9 981
khoshino 6:3792b68b1fc9 982 if(eps < 0){eps=qmax*mh*ueps;}
khoshino 6:3792b68b1fc9 983 rdet =1.0;
khoshino 6:3792b68b1fc9 984 idet =0.0;
khoshino 6:3792b68b1fc9 985 for(int k=0;k<mh;k++) {
khoshino 6:3792b68b1fc9 986 double amax = 0.0;
khoshino 6:3792b68b1fc9 987 int kmax = k;
khoshino 6:3792b68b1fc9 988 for(int i=k;i<mh;i++) {
khoshino 6:3792b68b1fc9 989 if(fabs(fpi[i][k])*wk[i] <= amax){}
khoshino 6:3792b68b1fc9 990 else{
khoshino 6:3792b68b1fc9 991 amax=fabs(fpi[i][k])*wk[i];
khoshino 6:3792b68b1fc9 992 kmax=i;
khoshino 6:3792b68b1fc9 993 }
khoshino 6:3792b68b1fc9 994 }
khoshino 6:3792b68b1fc9 995 // if(fabs(fpi[kmax][k]) <= eps){printf("MATRIX IS SINGULAR2\n");}
khoshino 6:3792b68b1fc9 996 rdet=rdet*fpi[kmax][k];
khoshino 6:3792b68b1fc9 997 if(k != kmax){rdet=-rdet;}
khoshino 6:3792b68b1fc9 998 double adet =fabs(rdet);
khoshino 6:3792b68b1fc9 999 while(adet > 1.0){
khoshino 6:3792b68b1fc9 1000 adet = adet*0.0625;
khoshino 6:3792b68b1fc9 1001 idet = idet + 4;
khoshino 6:3792b68b1fc9 1002 }
khoshino 6:3792b68b1fc9 1003 if(adet >= 0.0625){}
khoshino 6:3792b68b1fc9 1004 else{
khoshino 6:3792b68b1fc9 1005 adet=adet*16.0;
khoshino 6:3792b68b1fc9 1006 idet=idet-4;
khoshino 6:3792b68b1fc9 1007 }
khoshino 6:3792b68b1fc9 1008 if(rdet < 0.0){adet=-adet;}
khoshino 6:3792b68b1fc9 1009 rdet=adet;
khoshino 6:3792b68b1fc9 1010 iwk[k]=kmax;
khoshino 6:3792b68b1fc9 1011 amax=-1.0/fpi[kmax][k];
khoshino 6:3792b68b1fc9 1012 t=fpi[kmax][k];
khoshino 6:3792b68b1fc9 1013 fpi[kmax][k]=fpi[k][k];
khoshino 6:3792b68b1fc9 1014 fpi[k][k]=t;
khoshino 6:3792b68b1fc9 1015 for(int j=0;j<mh;j++) {
khoshino 6:3792b68b1fc9 1016 if(j == k){}
khoshino 6:3792b68b1fc9 1017 else{
khoshino 6:3792b68b1fc9 1018 t=fpi[kmax][j]*amax;
khoshino 6:3792b68b1fc9 1019 fpi[kmax][j]=fpi[k][j];
khoshino 6:3792b68b1fc9 1020 if(fabs(t) <= 0){}
khoshino 6:3792b68b1fc9 1021 else{
khoshino 6:3792b68b1fc9 1022 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 1023 fpi[i][j]=fpi[i][j]+t*fpi[i][k];
khoshino 6:3792b68b1fc9 1024 }
khoshino 6:3792b68b1fc9 1025 }
khoshino 6:3792b68b1fc9 1026 fpi[k][j]=-t;
khoshino 6:3792b68b1fc9 1027 }
khoshino 6:3792b68b1fc9 1028 }
khoshino 6:3792b68b1fc9 1029 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 1030 fpi[i][k]=fpi[i][k]*amax;
khoshino 6:3792b68b1fc9 1031 }
khoshino 6:3792b68b1fc9 1032 fpi[k][k]=-amax;
khoshino 6:3792b68b1fc9 1033 }
khoshino 6:3792b68b1fc9 1034
khoshino 6:3792b68b1fc9 1035 for(int l=0;l<mh;l++) {
khoshino 6:3792b68b1fc9 1036 int k=mh-l-1;
khoshino 6:3792b68b1fc9 1037 if(k == iwk[k]){}
khoshino 6:3792b68b1fc9 1038 else{
khoshino 6:3792b68b1fc9 1039 int kmax=iwk[k];
khoshino 6:3792b68b1fc9 1040 for(int i=0;i<mh;i++) {
khoshino 6:3792b68b1fc9 1041 t=fpi[i][k];
khoshino 6:3792b68b1fc9 1042 fpi[i][k]=fpi[i][kmax];
khoshino 6:3792b68b1fc9 1043 fpi[i][kmax]=t;
khoshino 6:3792b68b1fc9 1044 }
khoshino 6:3792b68b1fc9 1045 }
khoshino 6:3792b68b1fc9 1046 }
khoshino 6:3792b68b1fc9 1047
khoshino 6:3792b68b1fc9 1048 //////function
khoshino 6:3792b68b1fc9 1049 for(int jh=0;jh<mh;jh++){
khoshino 6:3792b68b1fc9 1050 sum=zer;
khoshino 6:3792b68b1fc9 1051 for(int ih=0;ih<mh;ih++){
khoshino 6:3792b68b1fc9 1052 sum=sum-fpi[jh][ih]*fn[ih];
khoshino 6:3792b68b1fc9 1053 }
khoshino 6:3792b68b1fc9 1054 dph4[jh]=sum;
khoshino 6:3792b68b1fc9 1055 }
khoshino 6:3792b68b1fc9 1056
khoshino 6:3792b68b1fc9 1057 //saitekikai
khoshino 6:3792b68b1fc9 1058
khoshino 6:3792b68b1fc9 1059 for(int it=0;it<mh;it++){
khoshino 6:3792b68b1fc9 1060 phf[it]= phf[it]+dt1*(dph1[it]+2*dph2[it]+2*dph3[it]+dph4[it])/6.0;
khoshino 6:3792b68b1fc9 1061 }
khoshino 6:3792b68b1fc9 1062
khoshino 6:3792b68b1fc9 1063 }
khoshino 6:3792b68b1fc9 1064
khoshino 6:3792b68b1fc9 1065 for (int id=0; id< mt; ++id){
khoshino 6:3792b68b1fc9 1066 int ii = mt-id-1;
khoshino 6:3792b68b1fc9 1067 int i0 = mv*ii;
khoshino 6:3792b68b1fc9 1068 ap[id]=phi[i0+0];
khoshino 6:3792b68b1fc9 1069 ps[id]=phi[i0+1];
khoshino 6:3792b68b1fc9 1070 xi1[id]=phi[i0+2]*drf;
khoshino 6:3792b68b1fc9 1071 et1[id]=phi[i0+3]*drf;
khoshino 6:3792b68b1fc9 1072 xi[id]=xi1[id]-xi1[0];
khoshino 6:3792b68b1fc9 1073 et[id]=et1[id]-et1[0];
khoshino 6:3792b68b1fc9 1074 }
khoshino 6:3792b68b1fc9 1075 led1 = 1;
khoshino 6:3792b68b1fc9 1076 wait(1.0);
khoshino 6:3792b68b1fc9 1077
khoshino 6:3792b68b1fc9 1078 ///////////////////
yal_kaiyo 9:a56c340f47c9 1079 flag_lc = 1; // end of homotopy flag
higedura 5:192835ac6106 1080 // ******************** homotopy ********************
higedura 5:192835ac6106 1081
higedura 5:192835ac6106 1082 }
higedura 5:192835ac6106 1083 }
higedura 5:192835ac6106 1084
higedura 5:192835ac6106 1085 while(flag_cm<loop){
higedura 0:9aa9be24636a 1086
higedura 0:9aa9be24636a 1087 // ******************** sequence No.1 ********************
higedura 0:9aa9be24636a 1088 buf_char = mavc.getc();
higedura 8:de86cf9ccb89 1089 buf_hex_rx[0] = (unsigned int)buf_char;
higedura 8:de86cf9ccb89 1090 for(int i=0;i<29;i++){ buf_hex_rx[29-i] = buf_hex_rx[28-i]; }
higedura 8:de86cf9ccb89 1091 if(buf_hex_rx[29]==0x40 && buf_hex_rx[28]==0x43 && buf_hex_rx[27]==0x4D && buf_hex_rx[26]==0x0D){
higedura 0:9aa9be24636a 1092 // ******************** sequence No.1 ********************
higedura 0:9aa9be24636a 1093
higedura 0:9aa9be24636a 1094 // ******************** sequence No.2 ********************
higedura 0:9aa9be24636a 1095 // send to mavc
higedura 0:9aa9be24636a 1096 mavc.putc(H);
higedura 0:9aa9be24636a 1097 for(int i=0;i<data;i++){ mavc.putc(D[i]); }
higedura 0:9aa9be24636a 1098 // ******************** sequence No.2 ********************
higedura 0:9aa9be24636a 1099
higedura 0:9aa9be24636a 1100 // ******************** sequence No.3 ********************
higedura 0:9aa9be24636a 1101 // buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 |
higedura 0:9aa9be24636a 1102 // |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY |
higedura 0:9aa9be24636a 1103 // | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] |
higedura 0:9aa9be24636a 1104 // transrating hex to dec
higedura 8:de86cf9ccb89 1105 for(int i=0;i<10;i++){ buf_dec_rx[i] = buf_hex_rx[24-i*2]*256+buf_hex_rx[23-i*2]; }
higedura 8:de86cf9ccb89 1106 for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec_rx[i]-32768)/65535*20*9.8; }
higedura 8:de86cf9ccb89 1107 for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec_rx[i+3]-32768)/65535*600/180*pi; }
higedura 8:de86cf9ccb89 1108 azi = (double)buf_dec_rx[6]/65535*2*pi;
higedura 8:de86cf9ccb89 1109 alt = ((double)buf_dec_rx[7]-32768)*0.1;
higedura 8:de86cf9ccb89 1110 for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec_rx[i+8]-32768)*0.1; }
higedura 0:9aa9be24636a 1111 // ******************** sequence No.3 ********************
higedura 0:9aa9be24636a 1112
khoshino 2:30f96c159d9c 1113 // ******************** initialization *******************
yal_kaiyo 9:a56c340f47c9 1114 for(int i=0;i<mt;i++){
yal_kaiyo 9:a56c340f47c9 1115 dr[i]=i*drf/md;
khoshino 2:30f96c159d9c 1116 }
yal_kaiyo 13:b444c425e194 1117 if(flag_int == 0){
yal_kaiyo 9:a56c340f47c9 1118 x0 = GPS[0];
yal_kaiyo 9:a56c340f47c9 1119 y0 = GPS[1];
yal_kaiyo 9:a56c340f47c9 1120 ps0 = azi;
yal_kaiyo 9:a56c340f47c9 1121 ht0 = alt;
yal_kaiyo 13:b444c425e194 1122 b0 = ht0;
yal_kaiyo 13:b444c425e194 1123 b1 = tan(gm0);
yal_kaiyo 13:b444c425e194 1124 b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf);
yal_kaiyo 13:b444c425e194 1125 b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
yal_kaiyo 13:b444c425e194 1126 flag_int=1;
yal_kaiyo 9:a56c340f47c9 1127 }
yal_kaiyo 14:feea7e2a51b8 1128 xis = (GPS[0] - x0)*cos(pi/2-ps0)+(GPS[1] - y0)*sin(pi/2-ps0);
yal_kaiyo 14:feea7e2a51b8 1129 ets = -(GPS[0] - x0)*cos(pi/2-ps0)+(GPS[1] - y0)*sin(pi/2-ps0);
yal_kaiyo 13:b444c425e194 1130 psr = rps0*dtr -(azi - ps0);
yal_kaiyo 13:b444c425e194 1131 htr = alt;
khoshino 4:037fab837823 1132 avp = gyro[0];
khoshino 4:037fab837823 1133 avq = gyro[1];
khoshino 4:037fab837823 1134 avr = gyro[2];
khoshino 2:30f96c159d9c 1135 // ******************** initialization *******************
khoshino 2:30f96c159d9c 1136
khoshino 2:30f96c159d9c 1137 // ******************** interpolation ********************
yal_kaiyo 14:feea7e2a51b8 1138 //xir ni hennkannseneba
yal_kaiyo 14:feea7e2a51b8 1139
yal_kaiyo 14:feea7e2a51b8 1140 //filter henko
khoshino 2:30f96c159d9c 1141
yal_kaiyo 9:a56c340f47c9 1142 if (xis != xis1 && fabs(xis-xis1)<50) {
khoshino 4:037fab837823 1143 s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1);
yal_kaiyo 9:a56c340f47c9 1144 ua = 0.5*(ua+sqrt(s)/(time-time1));
yal_kaiyo 9:a56c340f47c9 1145 drc = drc+ua*(time-time1);
yal_kaiyo 9:a56c340f47c9 1146 drp= drc;
yal_kaiyo 14:feea7e2a51b8 1147 xir=xis + ua * dt * cos(psr);
yal_kaiyo 14:feea7e2a51b8 1148 etr=ets + ua * dt * sin(psr);
yal_kaiyo 9:a56c340f47c9 1149 xis1=xis;
yal_kaiyo 9:a56c340f47c9 1150 ets1=ets;
khoshino 2:30f96c159d9c 1151 }
yal_kaiyo 14:feea7e2a51b8 1152 else{
yal_kaiyo 14:feea7e2a51b8 1153 drp = drp + ua * dt;
yal_kaiyo 14:feea7e2a51b8 1154 xir = xir + ua * dt * cos(psr);
yal_kaiyo 14:feea7e2a51b8 1155 etr = etr + ua * dt * sin(psr);
yal_kaiyo 14:feea7e2a51b8 1156 }
khoshino 2:30f96c159d9c 1157 // ******************** interpolation ********************
khoshino 2:30f96c159d9c 1158
khoshino 2:30f96c159d9c 1159 // ******************** guidance law WO homo *************
yal_kaiyo 13:b444c425e194 1160
yal_kaiyo 13:b444c425e194 1161 /*
yal_kaiyo 13:b444c425e194 1162 b0 = ht0
yal_kaiyo 13:b444c425e194 1163 b1 = (hf-ht0)/drf
yal_kaiyo 13:b444c425e194 1164 */
yal_kaiyo 14:feea7e2a51b8 1165 /*
yal_kaiyo 9:a56c340f47c9 1166 htc = b0 + b1*drp + b2*drp*drp + b3*drp*drp*drp;
khoshino 2:30f96c159d9c 1167
yal_kaiyo 9:a56c340f47c9 1168 for(int i=0;i<mt;i++){
khoshino 2:30f96c159d9c 1169 c = drp-dr[i];
khoshino 2:30f96c159d9c 1170 c1= dr[i+1]-drp;
khoshino 2:30f96c159d9c 1171
higedura 8:de86cf9ccb89 1172 if(c >=0 && c1 >= 0){
yal_kaiyo 9:a56c340f47c9 1173 c0=c/(dr[i+1]-dr[i]);
yal_kaiyo 9:a56c340f47c9 1174 c1=c1/(dr[i+1]-dr[i]);
yal_kaiyo 9:a56c340f47c9 1175 psc=c0*ps[i+1]+c1*ps[i];
yal_kaiyo 9:a56c340f47c9 1176 xic=c0*xi[i+1]+c1*xi[i];
yal_kaiyo 9:a56c340f47c9 1177 etc=c0*et[i+1]+c1*et[i];
yal_kaiyo 9:a56c340f47c9 1178 break;
khoshino 2:30f96c159d9c 1179 }
yal_kaiyo 9:a56c340f47c9 1180 }
yal_kaiyo 14:feea7e2a51b8 1181 */
yal_kaiyo 14:feea7e2a51b8 1182 htc = ht0;
yal_kaiyo 14:feea7e2a51b8 1183 xic = 0;
yal_kaiyo 14:feea7e2a51b8 1184 etc = 0;
khoshino 2:30f96c159d9c 1185 // ******************** guidance law WO homo *************
khoshino 2:30f96c159d9c 1186
khoshino 2:30f96c159d9c 1187 // ******************** control law **********************
yal_kaiyo 15:571b34671309 1188 xie=xic-xir;
kenkenpapurika 7:05a718fdef74 1189 pse=(psc-psr)*dtr;
kenkenpapurika 7:05a718fdef74 1190 ete=etc-etr;
kenkenpapurika 7:05a718fdef74 1191 hte=htc-htr;
kenkenpapurika 7:05a718fdef74 1192
kenkenpapurika 7:05a718fdef74 1193 //horizontal
yal_kaiyo 15:571b34671309 1194 v2=vmr*vmr;
yal_kaiyo 15:571b34671309 1195
kenkenpapurika 7:05a718fdef74 1196 gkp=omg1*omg1/grv;
kenkenpapurika 7:05a718fdef74 1197 gkd=(2*zet1*omg1*cos(gmr*dtr)/grv)*vmr;
yal_kaiyo 15:571b34671309 1198 phff=atan(apc*v2/grv);
yal_kaiyo 15:571b34671309 1199 phic=-gkp*ete-gkd*pse-phff;
yal_kaiyo 15:571b34671309 1200 avpc=(phic-phic00)/dt;
yal_kaiyo 15:571b34671309 1201 phic00=phic;
yal_kaiyo 9:a56c340f47c9 1202
yal_kaiyo 15:571b34671309 1203
kenkenpapurika 7:05a718fdef74 1204
kenkenpapurika 7:05a718fdef74 1205 //vertical
yal_kaiyo 15:571b34671309 1206
yal_kaiyo 15:571b34671309 1207 a22=grv*sin(gmr*dtr)/vmr;
yal_kaiyo 15:571b34671309 1208 a23=0;
yal_kaiyo 15:571b34671309 1209 a32=vmr*cos(gmr*dtr);
yal_kaiyo 15:571b34671309 1210 b21=swg*rho*vmr*dclda*cos(phir*dtr)/(2*wgt);
yal_kaiyo 15:571b34671309 1211 aaa=a32*b21;
yal_kaiyo 15:571b34671309 1212 ck1=(a22+2*zet2*omg2)/aaa;
yal_kaiyo 15:571b34671309 1213 ck2=(a23*a32+omg2*omg2)/aaa;
yal_kaiyo 15:571b34671309 1214
yal_kaiyo 15:571b34671309 1215
yal_kaiyo 15:571b34671309 1216
yal_kaiyo 15:571b34671309 1217
yal_kaiyo 15:571b34671309 1218 dhdt=(hte-hte00)/dt;
yal_kaiyo 15:571b34671309 1219
higedura 8:de86cf9ccb89 1220
higedura 8:de86cf9ccb89 1221 // to translate deg/s to rad/s, add *pi/180. TORATANI
yal_kaiyo 15:571b34671309 1222 avqc=(ck1*dhdt+ck2*hte)/tt2;
yal_kaiyo 15:571b34671309 1223 hte00=hte;
yal_kaiyo 9:a56c340f47c9 1224
yal_kaiyo 9:a56c340f47c9 1225
yal_kaiyo 14:feea7e2a51b8 1226
yal_kaiyo 13:b444c425e194 1227
yal_kaiyo 9:a56c340f47c9 1228 com[0] = avpc;
yal_kaiyo 9:a56c340f47c9 1229 com[1] = avqc;
yal_kaiyo 9:a56c340f47c9 1230 com[2] = avrc;
kenkenpapurika 7:05a718fdef74 1231
yal_kaiyo 15:571b34671309 1232 if(fabs(hte) > 12 || fabs(xie) > 12 || fabs(ete) >12){
yal_kaiyo 14:feea7e2a51b8 1233 com[0] = 0;
yal_kaiyo 14:feea7e2a51b8 1234 com[1] = 0;
yal_kaiyo 14:feea7e2a51b8 1235 com[2] = 0;
yal_kaiyo 14:feea7e2a51b8 1236 }
yal_kaiyo 12:33085c983354 1237
higedura 8:de86cf9ccb89 1238 // transrating dec to hex
higedura 8:de86cf9ccb89 1239 for(int i=0;i<3;i++){
higedura 8:de86cf9ccb89 1240 buf_dec_tx[i] = 19660.5/pi*com[i]+32768;
higedura 8:de86cf9ccb89 1241
yal_kaiyo 9:a56c340f47c9 1242 buf_hex_tx[i][0] = buf_dec_tx[i]/256;
yal_kaiyo 9:a56c340f47c9 1243 buf_dec_tx[i] = buf_dec_tx[i]-buf_hex_tx[i][0]*256;
yal_kaiyo 9:a56c340f47c9 1244 buf_hex_tx[i][1] = buf_dec_tx[i];
yal_kaiyo 9:a56c340f47c9 1245
yal_kaiyo 9:a56c340f47c9 1246 for(int j=0;j<2;j++){ D[j+2*i] = buf_hex_tx[i][j]; }
higedura 8:de86cf9ccb89 1247
higedura 8:de86cf9ccb89 1248 }
khoshino 2:30f96c159d9c 1249 // ******************** control law **********************
yal_kaiyo 9:a56c340f47c9 1250
higedura 3:17236ad0ff1e 1251 // !!! HOSHINO !!! fprintf add guidance, velocity and control
yal_kaiyo 12:33085c983354 1252 fprintf(fp, "%6.1f %7.3f %7.3f %7.3f %7.3f %7.3f %7.3f %8.5f %8.5f %8.1f %8.1f %8.1f %8.1f %8.1f %8.1f\r\n",time ,gyro[0],avpc, gyro[1],avqc, gyro[2],avrc, psr,psc, htr,htc, xir,xic,etr,etc);
yal_kaiyo 9:a56c340f47c9 1253
higedura 8:de86cf9ccb89 1254 time = time + 0.1;
higedura 5:192835ac6106 1255 flag_cm++;
khoshino 4:037fab837823 1256
higedura 8:de86cf9ccb89 1257 }
higedura 3:17236ad0ff1e 1258
higedura 0:9aa9be24636a 1259 }
higedura 0:9aa9be24636a 1260
higedura 0:9aa9be24636a 1261 fclose(fp);
higedura 0:9aa9be24636a 1262 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:9aa9be24636a 1263
higedura 0:9aa9be24636a 1264 }