CORE
ムササビチームの電装です
Dependencies: MPU6050 MS5607 SDFileSystem mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* 00002 2017年3月 伊豆大島共同打上実験 00003 団体名：CORE 00004 チーム名：ムササビ 00005 該当電装：ロケット搭載用 00006 00007 使用部品 00008 ・LPC1768(マイコン) 00009 ・MPU6050(加速度・ジャイロセンサ) 00010 ・MS5607(気圧・気温センサ) 00011 ・MicroSDスロット 00012 ・MG995(サーボモータ)×4 00013 00014 使用ライブラリ 00015 ・MPU6050.h 00016 https://developer.mbed.org/teams/mbed/code/SDFileSystem/ 00017 ・MS5607.h 00018 https://developer.mbed.org/users/yamaguch/code/MS5607/ 00019 ・SDFileSystem.h 00020 https://developer.mbed.org/teams/mbed/code/SDFileSystem/ 00021 */ 00022 #include "mbed.h" 00023 #include "math.h" 00024 #include "MS5607I2C.h" 00025 #include "MPU6050.h" 00026 #include "SDFileSystem.h" 00027 00028 #define ACC_LAUNCH 4.0f//FIXME:本番は4g 00029 #define TOP_DROP_AMOUNT 1.5f 00030 #define TIME_REACH_TOP 12.5f 00031 00032 #define RATE_LOG 1 00033 #define RATE_OPEN 10 00034 /*サーボ動作*/ 00035 #define LOCK 0 00036 #define UNLOCK 1 00037 /*モード定義*/ 00038 #define STANDBY 0 00039 #define TEST 1 00040 #define FLIGHT 2 00041 /*開放フェーズ定義*/ 00042 #define SETUP 0 00043 #define LAUNCH 1 00044 #define RISE 2 00045 #define DROP 3 00046 /**/ 00047 /*DCモータ動作*/ 00048 #define FORW 0 00049 #define STOP 1 00050 #define BACK 2 00051 #define P0 1013.25f//海面気圧[hPa] 00052 #define ACC 4096.0f//加速度オフセット値 00053 00054 /*目標地点の座標*/ 00055 #define L_HOKUI 34.73416 //ムササビチーム目標地点 00056 #define L_TOUKEI 139.4227 00057 #define ECC2 0.00669437999019758 00058 #define A_RADIUS 6378137.000 //長半径(赤道半径)[m] 00059 #define B_RADIUS 6356752.314245 //短半径(極半径)(WGS84)->http://yamadarake.jp/trdi/report000001.html 00060 #define M_PI 3.1415926535897932384626433832795 00061 #define M_PI_2 1.57079632679489661923 00062 #define epsilon 1.0E-6 00063 00064 /*ピン指定*/ 00065 MS5607I2C ms5607(p9, p10, false); 00066 MPU6050 mpu(p9,p10); 00067 BusOut myled(LED1,LED2,LED3,LED4); 00068 SDFileSystem sd(p5, p6, p7, p8, "sd"); 00069 Serial device(USBTX, USBRX); 00070 DigitalIn flightpin(p19); 00071 DigitalOut RED(p30); 00072 Serial twe(p13,p14); 00073 Serial gps(p28,p27); // tx, rx 00074 PwmOut Door_UP(p21); 00075 PwmOut Door_BOTTOM(p22); 00076 PwmOut DC(p23); 00077 00078 00079 /*タイマー類*/ 00080 Timer timer; 00081 Ticker loop_log; 00082 Ticker loop_open; 00083 /*ログカウンタ*/ 00084 bool row = 0; 00085 int8_t col = 0; 00086 /*ログ格納用*/ 00087 float alt_drop; 00088 FILE *fp; 00089 /*フェーズ変数*/ 00090 int8_t Phase = SETUP; 00091 int8_t Mode = STANDBY; 00092 char flight[5] = {}; 00093 int i_f =0; 00094 /*判定用*/ 00095 float alt_buff[RATE_OPEN]; 00096 static float alt_max,alt_launch; 00097 float t_drop,t_top,t_launch; 00098 int8_t cnt_judge = 0; 00099 int8_t col_open = 0; 00100 /*入力用*/ 00101 int8_t input_buff[2] = {}; 00102 int8_t input_cnt = 0; 00103 int u; 00104 00105 /*GPS変数*/ 00106 const float dt =0.06; 00107 int i,mode,Step; 00108 int j = 0; 00109 char gps_data[256]; 00110 char st,ns,ew; 00111 float w_time,hokui,tokei; 00112 float vel_norm,vel_head; 00113 00114 float g_hokui,g_tokei; 00115 float d_hokui,m_hokui,d_tokei,m_tokei; 00116 float def; 00117 unsigned char c; 00118 float dy, dx; 00119 float yAve,n,m,ecc,w; 00120 float xDist, yDist, dist; 00121 float x_tgt[3]= {166.7,-199.7,0.0}; 00122 00123 /*関数*/ 00124 void _Open(); 00125 //void _Log(); 00126 void _GPS(); 00127 void _Motor(int8_t door_num, int8_t motion); 00128 void _Input(); 00129 float _Measure_Alt(float press, float temp); 00130 float _Median(float data[], int num); 00131 int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max); 00132 int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt); 00133 void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[]); 00134 void periodize(float &x, float max, float min); 00135 float hdg_conv(float hdgN0deg); 00136 void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[]); 00137 void segmentBisector(float a[], float v1[], float v2[]); 00138 int intersection(float r[], float l[], float m[]); 00139 float dot2(float a[], float b[]); 00140 float norm2(float v[]); 00141 00142 /*---------------------------------------------------------------------*/ 00143 int main() { 00144 device.baud(115200); 00145 gps.baud(115200); 00146 twe.baud(115200); 00147 mpu.setAcceleroRange(2); 00148 mpu.setGyroRange(2); 00149 timer.start(); 00150 Door_UP.period_ms(20); 00151 Door_BOTTOM.period_ms(20); 00152 DC.period_ms(20); 00153 ms5607.printCoefficients(); 00154 00155 // _Motor(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定？ 00156 // _Motor(2,UNLOCK);// 00157 fp = fopen("/sd/log.txt", "a"); 00158 device.attach(&_Input,Serial::RxIrq); 00159 loop_open.attach(&_Open,1.0/RATE_OPEN); 00160 while(1); 00161 } 00162 /*開放用関数 RATE_OPEN[Hz]で判定を行う*/ 00163 void _Open(){ 00164 myled = 1 << (Phase-1); 00165 switch (Phase) { 00166 case SETUP://セットアップモード（発射判定不可） 00167 break; 00168 case LAUNCH://点火モード（発射判定可） 00169 device.printf("Phase:LAUNCH\r\n"); 00170 00171 float acc_buffx2 = (float)mpu.getAcceleroRawX()/(ACC*0.981); 00172 float acc_buffy2 = (float)mpu.getAcceleroRawY()/(ACC*0.981); 00173 float acc_buffz2 = (float)mpu.getAcceleroRawZ()/(ACC*0.981); 00174 00175 float x_acc=acc_buffx2*acc_buffx2; 00176 float y_acc=acc_buffy2*acc_buffy2; 00177 float z_acc=acc_buffz2*acc_buffz2; 00178 00179 float acc_sum = (x_acc+y_acc+z_acc)*1.0; 00180 alt_buff[col_open] = ms5607.getAltitude(); 00181 if(acc_sum>ACC_LAUNCH||flightpin==1){ 00182 if(cnt_judge++==3){ 00183 Phase = RISE; 00184 alt_launch = _Median(alt_buff, RATE_OPEN); 00185 cnt_judge = 0; 00186 } 00187 device.printf("launch:%f",alt_launch); 00188 t_launch = timer.read(); 00189 alt_max = alt_launch; 00190 }else{ 00191 if(timer.read()>t_launch+1.0) cnt_judge = 0; 00192 } 00193 break; 00194 case RISE://上昇中（パラシュート開放判定） 00195 device.printf("Phase:RISE\r\n"); 00196 float alt_rising = ms5607.getAltitude(); 00197 if( alt_rising>alt_max && alt_rising-alt_max < 10.0 ) alt_max = alt_rising; 00198 if(alt_rising<alt_max-TOP_DROP_AMOUNT){ 00199 if(cnt_judge++==3){ 00200 twe.printf("Phase:RISE.ALT Open.\r\n"); 00201 gps.attach(&_GPS,Serial::RxIrq); 00202 00203 _Motor(1,UNLOCK); 00204 Phase = DROP; 00205 cnt_judge = 0; 00206 } 00207 t_top = timer.read(); 00208 }else{ 00209 if(timer.read()>t_top+1.0) cnt_judge = 0; 00210 } 00211 if(timer.read()-t_launch>TIME_REACH_TOP){ 00212 _Motor(1,UNLOCK); 00213 gps.attach(&_GPS,Serial::RxIrq); 00214 00215 Phase = DROP; 00216 cnt_judge = 0; 00217 } 00218 break; 00219 case DROP://降下中（缶サット開放判定) 00220 00221 //device.printf("%d %c %f %f %f %f \n\r",u,st,vel_norm, vel_head, alt_drop, alt_max); 00222 //device.printf("Phase:DROP. Input:%d /r/n",u); 00223 00224 //_Measure alt 現在の高度 + dt->60ms 00225 00226 break; 00227 } 00228 if(col_open++==RATE_OPEN) col_open = 0; 00229 } 00230 /* 00231 void _Log(){ 00232 if(t[row][col] = timer.read()>=30.0*60.0){ 00233 timer.reset(); 00234 t[row][col] = timer.read(); 00235 } 00236 pressure[row][col] = ms5607.getPressure()/100; 00237 temperature[row][col] = ms5607.getTemperature(); 00238 fprintf(fp,"%d,%f,%f\r\n",t[row][col],pressure[row][col],temperature[row][col]); 00239 if(col++==RATE_LOG){ 00240 col = 0; 00241 row =! row; 00242 fclose(fp); 00243 fp = fopen("/sd/log.txt", "a"); 00244 } 00245 } 00246 */ 00247 void _Motor(int8_t num, int8_t motion) { 00248 if(num==1) { //扉 00249 if(motion==UNLOCK) { 00250 Door_UP.pulsewidth(0.0015); 00251 Door_BOTTOM.pulsewidth(0.0023); 00252 wait(1.0); 00253 Door_UP.pulsewidth(0); 00254 Door_BOTTOM.pulsewidth(0); 00255 } else if(motion==LOCK) { 00256 Door_UP.pulsewidth(0.0023); 00257 Door_BOTTOM.pulsewidth(0.0015); 00258 wait(1.0); 00259 Door_UP.pulsewidth(0); 00260 Door_BOTTOM.pulsewidth(0); 00261 } else { 00262 device.printf("error%f\r\n",motion); 00263 } 00264 } else if(num==2) { //DC 00265 if(motion==FORW) { 00266 DC.pulsewidth(0.001); 00267 wait(0.2); 00268 } else if(motion==STOP) { 00269 DC.pulsewidth(0.0015); 00270 wait(0.2); 00271 } else if(motion==BACK) { 00272 DC.pulsewidth(0.002); 00273 wait(0.2); 00274 } else { 00275 device.printf("error%f\r\n",motion); 00276 } 00277 } else { 00278 device.printf("Motor error:%d\r\n",num); 00279 } 00280 } 00281 void _GPS() { 00282 static int cnt=0; 00283 cnt++; 00284 RED =1; 00285 c = gps.getc(); 00286 if( c=='$' || i == 256){ 00287 mode = 0; 00288 i = 0; 00289 for(int j=0; j<256; j++){ 00290 gps_data[j]=NULL; 00291 } 00292 } 00293 if(mode==0){ 00294 if((gps_data[i]=c) != '\r'){ 00295 i++; 00296 }else{ 00297 gps_data[i]='\0'; 00298 00299 if( sscanf(gps_data, "$GNRMC,%f,%c,%f,%c,%f,%c,%f,%f",&w_time,&st,&hokui,&ns,&tokei,&ew,&vel_norm,&vel_head) >= 1){ 00300 //logitude 00301 00302 d_tokei= int(tokei/100); 00303 m_tokei= (tokei-d_tokei*100)/60; 00304 g_tokei= d_tokei+m_tokei; 00305 //Latitude 00306 d_hokui=int(hokui/100); 00307 m_hokui=(hokui-d_hokui*100)/60; 00308 g_hokui=d_hokui+m_hokui; 00309 00310 dy = (L_HOKUI-g_hokui)/180*M_PI; 00311 dx = (L_TOUKEI-g_tokei)/180*M_PI; 00312 yAve = (g_hokui+L_HOKUI)/2/180*M_PI; 00313 w = sqrt(1-ECC2*sin(yAve)*sin(yAve)); 00314 m = A_RADIUS*(1-ECC2)/(w*w*w); 00315 n = A_RADIUS/w; 00316 dist = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve))/1); 00317 xDist = dx*n*cos(yAve); 00318 yDist = dy*m; 00319 00320 float alt = ms5607.getAltitude(); 00321 //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head); 00322 device.printf("Radio \r\n"); 00323 00324 /* 00325 static int k=0; 00326 if (k++ % 6 == 0){ 00327 float alt = ms5607.getAltitude(); 00328 //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head); 00329 device.printf("Radio \r\n"); 00330 00331 twe.printf("%c,%f %f %f %f %f %d \r\n",Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u); 00332 fprintf(fp,"%c,%f %f %f %f %f %d \r\n" ,Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u); 00333 00334 if(k % 30 == 0){ 00335 k = 0; 00336 row =! row; 00337 fclose(fp); 00338 fp = fopen("/sd/log.txt", "a"); 00339 } 00340 } 00341 */ static float alt_drop2=ms5607.getAltitude(); 00342 alt_drop2=ms5607.getAltitude(); 00343 alt_drop=alt_drop2-alt_launch; 00344 00345 float x_s[] = { xDist, yDist }; 00346 device.printf("x_s %f %f drp:%f max:%f \r\n",x_s[0],x_s[1],alt_drop,alt_max - alt_launch); 00347 u = set_input(st == 'A', x_s, vel_norm, vel_head, alt_drop, alt_max - alt_launch); 00348 device.printf("%d",u); 00349 twe.printf("%d,%f %f %f %f %f %d \r\n",cnt,hokui,tokei,vel_norm,vel_head,alt_drop,alt_max - alt_launch,u); 00350 00351 static int ui = 0; 00352 const int ui_max = 30, ui_min = 10; // ui_min = 0 ? 00353 if (ui < ui_min) u = 1; 00354 else if (ui > ui_max) u = -1; 00355 ui += u; 00356 00357 if(u == -1) _Motor(2,FORW);//右旋回 00358 else if(u == 0) _Motor(2,STOP); 00359 else if(u == 1) _Motor(2,BACK);//左旋回 00360 00361 00362 sprintf(gps_data, ""); 00363 }//if 00364 } 00365 } 00366 } 00367 00368 void _Input(){ 00369 input_buff[input_cnt] = device.getc(); 00370 device.printf("\r\n"); 00371 switch (Mode) { 00372 case STANDBY: 00373 if(input_cnt==0){ 00374 if(input_buff[0]=='S'){ 00375 device.printf("U >> UNLOCK\r\n"); 00376 device.printf("L >> LOCK\r\n"); 00377 }else if(input_buff[0]=='M'){ 00378 device.printf("S >> STANDBY\r\n"); 00379 device.printf("F >> FLIGHT\r\n"); 00380 }else{ 00381 device.printf("This command is not found >> %c\r\n",input_buff[0]); 00382 device.printf(">>MAINMENU<<\r\n"); 00383 device.printf("S >> Servo Operation\r\n"); 00384 device.printf("M >> Mode Change\r\n"); 00385 device.printf("-->>"); 00386 return; 00387 } 00388 }else if(input_cnt==1){ 00389 if(input_buff[0]=='S'){ 00390 if(input_buff[1]=='U')_Motor(1,UNLOCK); 00391 else if(input_buff[1]=='L')_Motor(1,LOCK); 00392 else{ 00393 device.printf("This command is not found >> %c\r\n",input_buff[1]); 00394 device.printf("U >> UNLOCK\r\n"); 00395 device.printf("L >> LOCK\r\n"); 00396 device.printf("-->>"); 00397 return; 00398 } 00399 }else if(input_buff[0]=='M'){ 00400 if(input_buff[1]=='S'){ 00401 Mode = STANDBY; 00402 }else if(input_buff[1]=='F'){ 00403 Mode = FLIGHT; 00404 Phase = LAUNCH; 00405 00406 device.printf("FLIGHT-Mode ON!\r\n"); 00407 device.printf("***alert***\r\n"); 00408 device.printf("You will be able to reset only!\r\n"); 00409 return; 00410 }else{ 00411 device.printf("This command is not found >> %c\r\n",input_buff[1]); 00412 device.printf("S >> STANDBY\r\n"); 00413 device.printf("F >> FLIGHT\r\n"); 00414 device.printf("-->>"); 00415 return; 00416 } 00417 } 00418 input_cnt = 0; 00419 device.printf(">>MAINMENU<<\r\n"); 00420 device.printf("S >> Servo Operation\r\n"); 00421 device.printf("M >> Mode Change\r\n"); 00422 device.printf("-->>"); 00423 return; 00424 } 00425 device.printf("-->>"); 00426 input_cnt++; 00427 break; 00428 case FLIGHT://reset only 00429 break; 00430 } 00431 } 00432 00433 /*その他雑関数*/ 00434 float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){ 00435 return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; 00436 } 00437 float _Median(float data[], int num){ 00438 float median; 00439 float *sort = (float *)malloc(sizeof(float)*num); 00440 for(int i=0; i<num; i++) sort[i] = data[i]; 00441 for(int i=0; i<num; i++){ 00442 for(int j=0; j<num-i-1; j++){ 00443 if(sort[j]>sort[j+1]){ 00444 float buff = sort[j+1]; 00445 sort[j+1] = sort[j]; 00446 sort[j] = buff; 00447 } 00448 } 00449 } 00450 if(num%2!=0)median = sort[num/2]; 00451 else median = (sort[num/2-1]+sort[num/2])/2.0; 00452 free(sort); 00453 return median; 00454 } 00455 00456 int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max) 00457 { 00458 // グローバルにするかも？ 00459 const int dn = 40; // windstarのベクトルを取得するステップ間隔 00460 const float vz_est = 4.62; // パラフォイルのsinkrate[m/s] 00461 const float dist_tgt = 30.0; // 目標旋回半径[m] 00462 const float alt_unctrl = 50.0; // 無制御で降下する高度[m] 00463 00464 static int gps_step = 1; 00465 float u = 0.0; 00466 if (gps_good) 00467 { 00468 enum Phase 00469 { 00470 WIND_EST, 00471 CTRL 00472 }; 00473 static Phase phase = WIND_EST; 00474 00475 // sensor data 00476 float dt_ = dt*gps_step; 00477 gps_step = 1; 00478 00479 // state variable estimation 00480 float hdg_s = hdg_conv(hdg_gps); 00481 static float hdg0 = hdg_s; 00482 float omg_calc = hdg_s - hdg0; 00483 if (omg_calc >= M_PI) omg_calc -= 2 * M_PI; 00484 else if (omg_calc <= -M_PI) omg_calc += 2 * M_PI; 00485 omg_calc /= dt_; 00486 hdg0 = hdg_s; 00487 00488 // static float alt0 = alt_s; 00489 // float vz_calc = (alt_s - alt0) / dt_; 00490 // alt0 = alt_s; 00491 00492 float vg_s[] = { vg_knot*0.514444444*cos(hdg_s), vg_knot*0.514444444*sin(hdg_s) }; 00493 // if want to use estimated position, uncomment below 00494 // float x_est[2] = {}; 00495 // filter_x(dt_, 0.1, x_est, x_s, vg_s); 00496 00497 // wind estimation sequence 00498 static float vw_est[2] = {}; 00499 switch (phase) 00500 { 00501 case WIND_EST: 00502 { 00503 const int dn2 = 2 * dn; 00504 static int n = 0; 00505 float vw_t[2] = {}; 00506 static int cnt = 0; 00507 if (n >= dn2) 00508 { 00509 static int j = 0; 00510 static float vg_buf[dn2][2] = {}; 00511 windstar(vw_t, vg_buf[n % dn2], vg_buf[(n + dn) % dn2], vg_s); 00512 vw_est[0] = (vw_est[0] * j + vw_t[0]) / (j + 1); 00513 vw_est[1] = (vw_est[1] * j + vw_t[1]) / (j + 1); 00514 j++; 00515 vg_buf[n % dn2][0] = vg_s[0]; 00516 vg_buf[n % dn2][1] = vg_s[1]; 00517 } 00518 n++; 00519 00520 if (alt_s < alt_max - alt_unctrl) // 00521 if (cnt++ >= 3) phase = CTRL; 00522 } 00523 break; 00524 case CTRL: 00525 { 00526 float t_flt_est = (alt_s - x_tgt[2]) / vz_est; 00527 if (abs(vz_est) <= epsilon) t_flt_est = 0.0; 00528 float windoffset[2] = { -vw_est[0] * t_flt_est, -vw_est[1] * t_flt_est }; 00529 if (windoffset[1] > 200.0) windoffset[1] = 200.0; 00530 float x_virt[2] = { x_s[0] + windoffset[0], x_s[1] + windoffset[1] }; 00531 // if want to use estimated position, comment out above and uncomment below 00532 // float x_virt[2] = { x_est[0] + windoffset[0], x_est[1] + windoffset[1] }; 00533 float x_virt_rel[2] = { x_virt[0] - x_tgt[0], x_virt[1] - x_tgt[1] }; 00534 u = control(x_virt_rel, vw_est, vg_s, hdg_s, omg_calc, dist_tgt); 00535 } 00536 break; 00537 } 00538 00539 } 00540 else 00541 gps_step++; 00542 00543 return u; 00544 } 00545 00546 int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt) 00547 { 00548 const float threshold = 0.20; 00549 00550 float theta = atan2(r_rel[1], r_rel[0]); 00551 00552 float dist = norm2(r_rel); 00553 float Vr = dot2(r_rel, v) / dist; 00554 float Vtheta = (r_rel[0] * v[1] - r_rel[1] * v[0]) / dist; 00555 00556 float hdg_tgt = 0.0; 00557 float alpha = atan(0.5*(dist - dist_tgt)); 00558 float limitalpha = M_PI_2 - asin(dist_tgt / dist); 00559 if (alpha > limitalpha) alpha = limitalpha; 00560 if (Vtheta > 0) 00561 hdg_tgt = theta + M_PI_2 + alpha; 00562 else 00563 hdg_tgt = theta - M_PI_2 - alpha; 00564 periodize(hdg_tgt, M_PI, -M_PI); 00565 00566 float dhdg = hdg - hdg_tgt; 00567 periodize(dhdg, M_PI, -M_PI); 00568 00569 float domg = omg - Vtheta / dist; 00570 float Kp = 0.5, Kd = 3.0; 00571 float u = -Kp*dhdg - Kd*domg; 00572 00573 if (dist < 200.0) 00574 { 00575 if (Vtheta > 0) u += 0.05*Vr; 00576 else u -= 0.05*Vr; 00577 } 00578 00579 if (u > threshold) return 1; 00580 else if (u < -threshold) return -1; 00581 else return 0; 00582 } 00583 00584 void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[]) 00585 { 00586 static float x0[] = { x_gps[0], x_gps[1], x_gps[2] }; 00587 static int called = 0; 00588 if (!called) 00589 { 00590 for (int i = 0; i < 2; i++) 00591 x_est[i] = x0[i]; 00592 called++; 00593 return; 00594 } 00595 float xt[2]; 00596 for (int i = 0; i < 2; i++) 00597 { 00598 xt[i] = x0[i] + vg_gps[i] * dt; 00599 x_est[i] = C*x_gps[i] + (1 - C)*xt[i]; 00600 x0[i] = x_est[i]; 00601 } 00602 } 00603 00604 float hdg_conv(float hdgN0deg) 00605 { 00606 hdgN0deg *= -M_PI / 180.0; 00607 hdgN0deg += M_PI_2; 00608 periodize(hdgN0deg, M_PI, -M_PI); 00609 return hdgN0deg; 00610 } 00611 void periodize(float &x, float max, float min) 00612 { 00613 float range = max - min; 00614 while (x > max) x -= range; 00615 while (x < min) x += range; 00616 } 00617 00618 void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[]) 00619 { 00620 float l[3][3] = {}; // line coeffs l[0]*x + l[1]*y + l[2] = 0 00621 segmentBisector(l[0], Vg0, Vg1); 00622 segmentBisector(l[1], Vg1, Vg2); 00623 segmentBisector(l[2], Vg2, Vg0); 00624 float r[3][3] = {}; // intersection point 00625 intersection(r[0], l[0], l[1]); 00626 intersection(r[1], l[1], l[2]); 00627 intersection(r[2], l[2], l[0]); 00628 Vw[0] = (r[0][0] + r[1][0] + r[2][0]) / 3; 00629 Vw[1] = (r[0][1] + r[1][1] + r[2][1]) / 3; 00630 } 00631 void segmentBisector(float a[], float v1[], float v2[]) 00632 { 00633 a[0] = v2[0] - v1[0]; 00634 a[1] = v2[1] - v1[1]; 00635 a[2] = -0.5*(a[0] * (v1[0] + v2[0]) + a[1] * (v1[1] + v2[1])); 00636 } 00637 int intersection(float r[], float l[], float m[]) 00638 { 00639 float det = l[0] * m[1] - l[1] * m[0]; 00640 if (abs(det) < epsilon) return -1; 00641 r[0] = (-m[1] * l[2] + l[1] * m[2]) / det; 00642 r[1] = (m[0] * l[2] - l[0] * m[2]) / det; 00643 return 0; 00644 } 00645 00646 float dot2(float a[], float b[]) 00647 { 00648 return a[0] * b[0] + a[1] * b[1]; 00649 } 00650 float norm2(float v[]) 00651 { 00652 return sqrt(dot2(v, v)); 00653 }
Generated on Wed Jul 13 2022 13:23:27 by
1.7.2