yal kaiyo / Mbed 2 deprecated YNU_MPU_tora

for hige

Fork of YNU_MPU_1 by yal kaiyo

main.cpp@17:3cd1afc27ca7, 2013-01-20 (annotated)

Committer:
yal_kaiyo
Date:
Sun Jan 20 13:38:22 2013 +0000
Revision:
17:3cd1afc27ca7
Parent:
16:a33fe7d99a0f 
for demonstration

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