yal kaiyo
for demonstration
Dependencies: FatFileSystem SDFileSystem mbed
Fork of
130114_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?
| User | Revision | Line number | New 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 | } |