ムササビチームの電装です
Dependencies: MPU6050 MS5607 SDFileSystem mbed
main.cpp@0:b021d725d528, 2017-04-05 (annotated)
- Committer:
- makakensanba
- Date:
- Wed Apr 05 10:45:35 2017 +0000
- Revision:
- 0:b021d725d528
?????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
makakensanba | 0:b021d725d528 | 1 | /* |
makakensanba | 0:b021d725d528 | 2 | 2017年3月 伊豆大島共同打上実験 |
makakensanba | 0:b021d725d528 | 3 | 団体名:CORE |
makakensanba | 0:b021d725d528 | 4 | チーム名:ムササビ |
makakensanba | 0:b021d725d528 | 5 | 該当電装:ロケット搭載用 |
makakensanba | 0:b021d725d528 | 6 | |
makakensanba | 0:b021d725d528 | 7 | 使用部品 |
makakensanba | 0:b021d725d528 | 8 | ・LPC1768(マイコン) |
makakensanba | 0:b021d725d528 | 9 | ・MPU6050(加速度・ジャイロセンサ) |
makakensanba | 0:b021d725d528 | 10 | ・MS5607(気圧・気温センサ) |
makakensanba | 0:b021d725d528 | 11 | ・MicroSDスロット |
makakensanba | 0:b021d725d528 | 12 | ・MG995(サーボモータ)×4 |
makakensanba | 0:b021d725d528 | 13 | |
makakensanba | 0:b021d725d528 | 14 | 使用ライブラリ |
makakensanba | 0:b021d725d528 | 15 | ・MPU6050.h |
makakensanba | 0:b021d725d528 | 16 | https://developer.mbed.org/teams/mbed/code/SDFileSystem/ |
makakensanba | 0:b021d725d528 | 17 | ・MS5607.h |
makakensanba | 0:b021d725d528 | 18 | https://developer.mbed.org/users/yamaguch/code/MS5607/ |
makakensanba | 0:b021d725d528 | 19 | ・SDFileSystem.h |
makakensanba | 0:b021d725d528 | 20 | https://developer.mbed.org/teams/mbed/code/SDFileSystem/ |
makakensanba | 0:b021d725d528 | 21 | */ |
makakensanba | 0:b021d725d528 | 22 | #include "mbed.h" |
makakensanba | 0:b021d725d528 | 23 | #include "math.h" |
makakensanba | 0:b021d725d528 | 24 | #include "MS5607I2C.h" |
makakensanba | 0:b021d725d528 | 25 | #include "MPU6050.h" |
makakensanba | 0:b021d725d528 | 26 | #include "SDFileSystem.h" |
makakensanba | 0:b021d725d528 | 27 | |
makakensanba | 0:b021d725d528 | 28 | #define ACC_LAUNCH 4.0f//FIXME:本番は4g |
makakensanba | 0:b021d725d528 | 29 | #define TOP_DROP_AMOUNT 1.5f |
makakensanba | 0:b021d725d528 | 30 | #define TIME_REACH_TOP 12.5f |
makakensanba | 0:b021d725d528 | 31 | |
makakensanba | 0:b021d725d528 | 32 | #define RATE_LOG 1 |
makakensanba | 0:b021d725d528 | 33 | #define RATE_OPEN 10 |
makakensanba | 0:b021d725d528 | 34 | /*サーボ動作*/ |
makakensanba | 0:b021d725d528 | 35 | #define LOCK 0 |
makakensanba | 0:b021d725d528 | 36 | #define UNLOCK 1 |
makakensanba | 0:b021d725d528 | 37 | /*モード定義*/ |
makakensanba | 0:b021d725d528 | 38 | #define STANDBY 0 |
makakensanba | 0:b021d725d528 | 39 | #define TEST 1 |
makakensanba | 0:b021d725d528 | 40 | #define FLIGHT 2 |
makakensanba | 0:b021d725d528 | 41 | /*開放フェーズ定義*/ |
makakensanba | 0:b021d725d528 | 42 | #define SETUP 0 |
makakensanba | 0:b021d725d528 | 43 | #define LAUNCH 1 |
makakensanba | 0:b021d725d528 | 44 | #define RISE 2 |
makakensanba | 0:b021d725d528 | 45 | #define DROP 3 |
makakensanba | 0:b021d725d528 | 46 | /**/ |
makakensanba | 0:b021d725d528 | 47 | /*DCモータ動作*/ |
makakensanba | 0:b021d725d528 | 48 | #define FORW 0 |
makakensanba | 0:b021d725d528 | 49 | #define STOP 1 |
makakensanba | 0:b021d725d528 | 50 | #define BACK 2 |
makakensanba | 0:b021d725d528 | 51 | #define P0 1013.25f//海面気圧[hPa] |
makakensanba | 0:b021d725d528 | 52 | #define ACC 4096.0f//加速度オフセット値 |
makakensanba | 0:b021d725d528 | 53 | |
makakensanba | 0:b021d725d528 | 54 | /*目標地点の座標*/ |
makakensanba | 0:b021d725d528 | 55 | #define L_HOKUI 34.73416 //ムササビチーム目標地点 |
makakensanba | 0:b021d725d528 | 56 | #define L_TOUKEI 139.4227 |
makakensanba | 0:b021d725d528 | 57 | #define ECC2 0.00669437999019758 |
makakensanba | 0:b021d725d528 | 58 | #define A_RADIUS 6378137.000 //長半径(赤道半径)[m] |
makakensanba | 0:b021d725d528 | 59 | #define B_RADIUS 6356752.314245 //短半径(極半径)(WGS84)->http://yamadarake.jp/trdi/report000001.html |
makakensanba | 0:b021d725d528 | 60 | #define M_PI 3.1415926535897932384626433832795 |
makakensanba | 0:b021d725d528 | 61 | #define M_PI_2 1.57079632679489661923 |
makakensanba | 0:b021d725d528 | 62 | #define epsilon 1.0E-6 |
makakensanba | 0:b021d725d528 | 63 | |
makakensanba | 0:b021d725d528 | 64 | /*ピン指定*/ |
makakensanba | 0:b021d725d528 | 65 | MS5607I2C ms5607(p9, p10, false); |
makakensanba | 0:b021d725d528 | 66 | MPU6050 mpu(p9,p10); |
makakensanba | 0:b021d725d528 | 67 | BusOut myled(LED1,LED2,LED3,LED4); |
makakensanba | 0:b021d725d528 | 68 | SDFileSystem sd(p5, p6, p7, p8, "sd"); |
makakensanba | 0:b021d725d528 | 69 | Serial device(USBTX, USBRX); |
makakensanba | 0:b021d725d528 | 70 | DigitalIn flightpin(p19); |
makakensanba | 0:b021d725d528 | 71 | DigitalOut RED(p30); |
makakensanba | 0:b021d725d528 | 72 | Serial twe(p13,p14); |
makakensanba | 0:b021d725d528 | 73 | Serial gps(p28,p27); // tx, rx |
makakensanba | 0:b021d725d528 | 74 | PwmOut Door_UP(p21); |
makakensanba | 0:b021d725d528 | 75 | PwmOut Door_BOTTOM(p22); |
makakensanba | 0:b021d725d528 | 76 | PwmOut DC(p23); |
makakensanba | 0:b021d725d528 | 77 | |
makakensanba | 0:b021d725d528 | 78 | |
makakensanba | 0:b021d725d528 | 79 | /*タイマー類*/ |
makakensanba | 0:b021d725d528 | 80 | Timer timer; |
makakensanba | 0:b021d725d528 | 81 | Ticker loop_log; |
makakensanba | 0:b021d725d528 | 82 | Ticker loop_open; |
makakensanba | 0:b021d725d528 | 83 | /*ログカウンタ*/ |
makakensanba | 0:b021d725d528 | 84 | bool row = 0; |
makakensanba | 0:b021d725d528 | 85 | int8_t col = 0; |
makakensanba | 0:b021d725d528 | 86 | /*ログ格納用*/ |
makakensanba | 0:b021d725d528 | 87 | float alt_drop; |
makakensanba | 0:b021d725d528 | 88 | FILE *fp; |
makakensanba | 0:b021d725d528 | 89 | /*フェーズ変数*/ |
makakensanba | 0:b021d725d528 | 90 | int8_t Phase = SETUP; |
makakensanba | 0:b021d725d528 | 91 | int8_t Mode = STANDBY; |
makakensanba | 0:b021d725d528 | 92 | char flight[5] = {}; |
makakensanba | 0:b021d725d528 | 93 | int i_f =0; |
makakensanba | 0:b021d725d528 | 94 | /*判定用*/ |
makakensanba | 0:b021d725d528 | 95 | float alt_buff[RATE_OPEN]; |
makakensanba | 0:b021d725d528 | 96 | static float alt_max,alt_launch; |
makakensanba | 0:b021d725d528 | 97 | float t_drop,t_top,t_launch; |
makakensanba | 0:b021d725d528 | 98 | int8_t cnt_judge = 0; |
makakensanba | 0:b021d725d528 | 99 | int8_t col_open = 0; |
makakensanba | 0:b021d725d528 | 100 | /*入力用*/ |
makakensanba | 0:b021d725d528 | 101 | int8_t input_buff[2] = {}; |
makakensanba | 0:b021d725d528 | 102 | int8_t input_cnt = 0; |
makakensanba | 0:b021d725d528 | 103 | int u; |
makakensanba | 0:b021d725d528 | 104 | |
makakensanba | 0:b021d725d528 | 105 | /*GPS変数*/ |
makakensanba | 0:b021d725d528 | 106 | const float dt =0.06; |
makakensanba | 0:b021d725d528 | 107 | int i,mode,Step; |
makakensanba | 0:b021d725d528 | 108 | int j = 0; |
makakensanba | 0:b021d725d528 | 109 | char gps_data[256]; |
makakensanba | 0:b021d725d528 | 110 | char st,ns,ew; |
makakensanba | 0:b021d725d528 | 111 | float w_time,hokui,tokei; |
makakensanba | 0:b021d725d528 | 112 | float vel_norm,vel_head; |
makakensanba | 0:b021d725d528 | 113 | |
makakensanba | 0:b021d725d528 | 114 | float g_hokui,g_tokei; |
makakensanba | 0:b021d725d528 | 115 | float d_hokui,m_hokui,d_tokei,m_tokei; |
makakensanba | 0:b021d725d528 | 116 | float def; |
makakensanba | 0:b021d725d528 | 117 | unsigned char c; |
makakensanba | 0:b021d725d528 | 118 | float dy, dx; |
makakensanba | 0:b021d725d528 | 119 | float yAve,n,m,ecc,w; |
makakensanba | 0:b021d725d528 | 120 | float xDist, yDist, dist; |
makakensanba | 0:b021d725d528 | 121 | float x_tgt[3]= {166.7,-199.7,0.0}; |
makakensanba | 0:b021d725d528 | 122 | |
makakensanba | 0:b021d725d528 | 123 | /*関数*/ |
makakensanba | 0:b021d725d528 | 124 | void _Open(); |
makakensanba | 0:b021d725d528 | 125 | //void _Log(); |
makakensanba | 0:b021d725d528 | 126 | void _GPS(); |
makakensanba | 0:b021d725d528 | 127 | void _Motor(int8_t door_num, int8_t motion); |
makakensanba | 0:b021d725d528 | 128 | void _Input(); |
makakensanba | 0:b021d725d528 | 129 | float _Measure_Alt(float press, float temp); |
makakensanba | 0:b021d725d528 | 130 | float _Median(float data[], int num); |
makakensanba | 0:b021d725d528 | 131 | int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max); |
makakensanba | 0:b021d725d528 | 132 | int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt); |
makakensanba | 0:b021d725d528 | 133 | void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[]); |
makakensanba | 0:b021d725d528 | 134 | void periodize(float &x, float max, float min); |
makakensanba | 0:b021d725d528 | 135 | float hdg_conv(float hdgN0deg); |
makakensanba | 0:b021d725d528 | 136 | void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[]); |
makakensanba | 0:b021d725d528 | 137 | void segmentBisector(float a[], float v1[], float v2[]); |
makakensanba | 0:b021d725d528 | 138 | int intersection(float r[], float l[], float m[]); |
makakensanba | 0:b021d725d528 | 139 | float dot2(float a[], float b[]); |
makakensanba | 0:b021d725d528 | 140 | float norm2(float v[]); |
makakensanba | 0:b021d725d528 | 141 | |
makakensanba | 0:b021d725d528 | 142 | /*---------------------------------------------------------------------*/ |
makakensanba | 0:b021d725d528 | 143 | int main() { |
makakensanba | 0:b021d725d528 | 144 | device.baud(115200); |
makakensanba | 0:b021d725d528 | 145 | gps.baud(115200); |
makakensanba | 0:b021d725d528 | 146 | twe.baud(115200); |
makakensanba | 0:b021d725d528 | 147 | mpu.setAcceleroRange(2); |
makakensanba | 0:b021d725d528 | 148 | mpu.setGyroRange(2); |
makakensanba | 0:b021d725d528 | 149 | timer.start(); |
makakensanba | 0:b021d725d528 | 150 | Door_UP.period_ms(20); |
makakensanba | 0:b021d725d528 | 151 | Door_BOTTOM.period_ms(20); |
makakensanba | 0:b021d725d528 | 152 | DC.period_ms(20); |
makakensanba | 0:b021d725d528 | 153 | ms5607.printCoefficients(); |
makakensanba | 0:b021d725d528 | 154 | |
makakensanba | 0:b021d725d528 | 155 | // _Motor(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定? |
makakensanba | 0:b021d725d528 | 156 | // _Motor(2,UNLOCK);// |
makakensanba | 0:b021d725d528 | 157 | fp = fopen("/sd/log.txt", "a"); |
makakensanba | 0:b021d725d528 | 158 | device.attach(&_Input,Serial::RxIrq); |
makakensanba | 0:b021d725d528 | 159 | loop_open.attach(&_Open,1.0/RATE_OPEN); |
makakensanba | 0:b021d725d528 | 160 | while(1); |
makakensanba | 0:b021d725d528 | 161 | } |
makakensanba | 0:b021d725d528 | 162 | /*開放用関数 RATE_OPEN[Hz]で判定を行う*/ |
makakensanba | 0:b021d725d528 | 163 | void _Open(){ |
makakensanba | 0:b021d725d528 | 164 | myled = 1 << (Phase-1); |
makakensanba | 0:b021d725d528 | 165 | switch (Phase) { |
makakensanba | 0:b021d725d528 | 166 | case SETUP://セットアップモード(発射判定不可) |
makakensanba | 0:b021d725d528 | 167 | break; |
makakensanba | 0:b021d725d528 | 168 | case LAUNCH://点火モード(発射判定可) |
makakensanba | 0:b021d725d528 | 169 | device.printf("Phase:LAUNCH\r\n"); |
makakensanba | 0:b021d725d528 | 170 | |
makakensanba | 0:b021d725d528 | 171 | float acc_buffx2 = (float)mpu.getAcceleroRawX()/(ACC*0.981); |
makakensanba | 0:b021d725d528 | 172 | float acc_buffy2 = (float)mpu.getAcceleroRawY()/(ACC*0.981); |
makakensanba | 0:b021d725d528 | 173 | float acc_buffz2 = (float)mpu.getAcceleroRawZ()/(ACC*0.981); |
makakensanba | 0:b021d725d528 | 174 | |
makakensanba | 0:b021d725d528 | 175 | float x_acc=acc_buffx2*acc_buffx2; |
makakensanba | 0:b021d725d528 | 176 | float y_acc=acc_buffy2*acc_buffy2; |
makakensanba | 0:b021d725d528 | 177 | float z_acc=acc_buffz2*acc_buffz2; |
makakensanba | 0:b021d725d528 | 178 | |
makakensanba | 0:b021d725d528 | 179 | float acc_sum = (x_acc+y_acc+z_acc)*1.0; |
makakensanba | 0:b021d725d528 | 180 | alt_buff[col_open] = ms5607.getAltitude(); |
makakensanba | 0:b021d725d528 | 181 | if(acc_sum>ACC_LAUNCH||flightpin==1){ |
makakensanba | 0:b021d725d528 | 182 | if(cnt_judge++==3){ |
makakensanba | 0:b021d725d528 | 183 | Phase = RISE; |
makakensanba | 0:b021d725d528 | 184 | alt_launch = _Median(alt_buff, RATE_OPEN); |
makakensanba | 0:b021d725d528 | 185 | cnt_judge = 0; |
makakensanba | 0:b021d725d528 | 186 | } |
makakensanba | 0:b021d725d528 | 187 | device.printf("launch:%f",alt_launch); |
makakensanba | 0:b021d725d528 | 188 | t_launch = timer.read(); |
makakensanba | 0:b021d725d528 | 189 | alt_max = alt_launch; |
makakensanba | 0:b021d725d528 | 190 | }else{ |
makakensanba | 0:b021d725d528 | 191 | if(timer.read()>t_launch+1.0) cnt_judge = 0; |
makakensanba | 0:b021d725d528 | 192 | } |
makakensanba | 0:b021d725d528 | 193 | break; |
makakensanba | 0:b021d725d528 | 194 | case RISE://上昇中(パラシュート開放判定) |
makakensanba | 0:b021d725d528 | 195 | device.printf("Phase:RISE\r\n"); |
makakensanba | 0:b021d725d528 | 196 | float alt_rising = ms5607.getAltitude(); |
makakensanba | 0:b021d725d528 | 197 | if( alt_rising>alt_max && alt_rising-alt_max < 10.0 ) alt_max = alt_rising; |
makakensanba | 0:b021d725d528 | 198 | if(alt_rising<alt_max-TOP_DROP_AMOUNT){ |
makakensanba | 0:b021d725d528 | 199 | if(cnt_judge++==3){ |
makakensanba | 0:b021d725d528 | 200 | twe.printf("Phase:RISE.ALT Open.\r\n"); |
makakensanba | 0:b021d725d528 | 201 | gps.attach(&_GPS,Serial::RxIrq); |
makakensanba | 0:b021d725d528 | 202 | |
makakensanba | 0:b021d725d528 | 203 | _Motor(1,UNLOCK); |
makakensanba | 0:b021d725d528 | 204 | Phase = DROP; |
makakensanba | 0:b021d725d528 | 205 | cnt_judge = 0; |
makakensanba | 0:b021d725d528 | 206 | } |
makakensanba | 0:b021d725d528 | 207 | t_top = timer.read(); |
makakensanba | 0:b021d725d528 | 208 | }else{ |
makakensanba | 0:b021d725d528 | 209 | if(timer.read()>t_top+1.0) cnt_judge = 0; |
makakensanba | 0:b021d725d528 | 210 | } |
makakensanba | 0:b021d725d528 | 211 | if(timer.read()-t_launch>TIME_REACH_TOP){ |
makakensanba | 0:b021d725d528 | 212 | _Motor(1,UNLOCK); |
makakensanba | 0:b021d725d528 | 213 | gps.attach(&_GPS,Serial::RxIrq); |
makakensanba | 0:b021d725d528 | 214 | |
makakensanba | 0:b021d725d528 | 215 | Phase = DROP; |
makakensanba | 0:b021d725d528 | 216 | cnt_judge = 0; |
makakensanba | 0:b021d725d528 | 217 | } |
makakensanba | 0:b021d725d528 | 218 | break; |
makakensanba | 0:b021d725d528 | 219 | case DROP://降下中(缶サット開放判定) |
makakensanba | 0:b021d725d528 | 220 | |
makakensanba | 0:b021d725d528 | 221 | //device.printf("%d %c %f %f %f %f \n\r",u,st,vel_norm, vel_head, alt_drop, alt_max); |
makakensanba | 0:b021d725d528 | 222 | //device.printf("Phase:DROP. Input:%d /r/n",u); |
makakensanba | 0:b021d725d528 | 223 | |
makakensanba | 0:b021d725d528 | 224 | //_Measure alt 現在の高度 + dt->60ms |
makakensanba | 0:b021d725d528 | 225 | |
makakensanba | 0:b021d725d528 | 226 | break; |
makakensanba | 0:b021d725d528 | 227 | } |
makakensanba | 0:b021d725d528 | 228 | if(col_open++==RATE_OPEN) col_open = 0; |
makakensanba | 0:b021d725d528 | 229 | } |
makakensanba | 0:b021d725d528 | 230 | /* |
makakensanba | 0:b021d725d528 | 231 | void _Log(){ |
makakensanba | 0:b021d725d528 | 232 | if(t[row][col] = timer.read()>=30.0*60.0){ |
makakensanba | 0:b021d725d528 | 233 | timer.reset(); |
makakensanba | 0:b021d725d528 | 234 | t[row][col] = timer.read(); |
makakensanba | 0:b021d725d528 | 235 | } |
makakensanba | 0:b021d725d528 | 236 | pressure[row][col] = ms5607.getPressure()/100; |
makakensanba | 0:b021d725d528 | 237 | temperature[row][col] = ms5607.getTemperature(); |
makakensanba | 0:b021d725d528 | 238 | fprintf(fp,"%d,%f,%f\r\n",t[row][col],pressure[row][col],temperature[row][col]); |
makakensanba | 0:b021d725d528 | 239 | if(col++==RATE_LOG){ |
makakensanba | 0:b021d725d528 | 240 | col = 0; |
makakensanba | 0:b021d725d528 | 241 | row =! row; |
makakensanba | 0:b021d725d528 | 242 | fclose(fp); |
makakensanba | 0:b021d725d528 | 243 | fp = fopen("/sd/log.txt", "a"); |
makakensanba | 0:b021d725d528 | 244 | } |
makakensanba | 0:b021d725d528 | 245 | } |
makakensanba | 0:b021d725d528 | 246 | */ |
makakensanba | 0:b021d725d528 | 247 | void _Motor(int8_t num, int8_t motion) { |
makakensanba | 0:b021d725d528 | 248 | if(num==1) { //扉 |
makakensanba | 0:b021d725d528 | 249 | if(motion==UNLOCK) { |
makakensanba | 0:b021d725d528 | 250 | Door_UP.pulsewidth(0.0015); |
makakensanba | 0:b021d725d528 | 251 | Door_BOTTOM.pulsewidth(0.0023); |
makakensanba | 0:b021d725d528 | 252 | wait(1.0); |
makakensanba | 0:b021d725d528 | 253 | Door_UP.pulsewidth(0); |
makakensanba | 0:b021d725d528 | 254 | Door_BOTTOM.pulsewidth(0); |
makakensanba | 0:b021d725d528 | 255 | } else if(motion==LOCK) { |
makakensanba | 0:b021d725d528 | 256 | Door_UP.pulsewidth(0.0023); |
makakensanba | 0:b021d725d528 | 257 | Door_BOTTOM.pulsewidth(0.0015); |
makakensanba | 0:b021d725d528 | 258 | wait(1.0); |
makakensanba | 0:b021d725d528 | 259 | Door_UP.pulsewidth(0); |
makakensanba | 0:b021d725d528 | 260 | Door_BOTTOM.pulsewidth(0); |
makakensanba | 0:b021d725d528 | 261 | } else { |
makakensanba | 0:b021d725d528 | 262 | device.printf("error%f\r\n",motion); |
makakensanba | 0:b021d725d528 | 263 | } |
makakensanba | 0:b021d725d528 | 264 | } else if(num==2) { //DC |
makakensanba | 0:b021d725d528 | 265 | if(motion==FORW) { |
makakensanba | 0:b021d725d528 | 266 | DC.pulsewidth(0.001); |
makakensanba | 0:b021d725d528 | 267 | wait(0.2); |
makakensanba | 0:b021d725d528 | 268 | } else if(motion==STOP) { |
makakensanba | 0:b021d725d528 | 269 | DC.pulsewidth(0.0015); |
makakensanba | 0:b021d725d528 | 270 | wait(0.2); |
makakensanba | 0:b021d725d528 | 271 | } else if(motion==BACK) { |
makakensanba | 0:b021d725d528 | 272 | DC.pulsewidth(0.002); |
makakensanba | 0:b021d725d528 | 273 | wait(0.2); |
makakensanba | 0:b021d725d528 | 274 | } else { |
makakensanba | 0:b021d725d528 | 275 | device.printf("error%f\r\n",motion); |
makakensanba | 0:b021d725d528 | 276 | } |
makakensanba | 0:b021d725d528 | 277 | } else { |
makakensanba | 0:b021d725d528 | 278 | device.printf("Motor error:%d\r\n",num); |
makakensanba | 0:b021d725d528 | 279 | } |
makakensanba | 0:b021d725d528 | 280 | } |
makakensanba | 0:b021d725d528 | 281 | void _GPS() { |
makakensanba | 0:b021d725d528 | 282 | static int cnt=0; |
makakensanba | 0:b021d725d528 | 283 | cnt++; |
makakensanba | 0:b021d725d528 | 284 | RED =1; |
makakensanba | 0:b021d725d528 | 285 | c = gps.getc(); |
makakensanba | 0:b021d725d528 | 286 | if( c=='$' || i == 256){ |
makakensanba | 0:b021d725d528 | 287 | mode = 0; |
makakensanba | 0:b021d725d528 | 288 | i = 0; |
makakensanba | 0:b021d725d528 | 289 | for(int j=0; j<256; j++){ |
makakensanba | 0:b021d725d528 | 290 | gps_data[j]=NULL; |
makakensanba | 0:b021d725d528 | 291 | } |
makakensanba | 0:b021d725d528 | 292 | } |
makakensanba | 0:b021d725d528 | 293 | if(mode==0){ |
makakensanba | 0:b021d725d528 | 294 | if((gps_data[i]=c) != '\r'){ |
makakensanba | 0:b021d725d528 | 295 | i++; |
makakensanba | 0:b021d725d528 | 296 | }else{ |
makakensanba | 0:b021d725d528 | 297 | gps_data[i]='\0'; |
makakensanba | 0:b021d725d528 | 298 | |
makakensanba | 0:b021d725d528 | 299 | 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){ |
makakensanba | 0:b021d725d528 | 300 | //logitude |
makakensanba | 0:b021d725d528 | 301 | |
makakensanba | 0:b021d725d528 | 302 | d_tokei= int(tokei/100); |
makakensanba | 0:b021d725d528 | 303 | m_tokei= (tokei-d_tokei*100)/60; |
makakensanba | 0:b021d725d528 | 304 | g_tokei= d_tokei+m_tokei; |
makakensanba | 0:b021d725d528 | 305 | //Latitude |
makakensanba | 0:b021d725d528 | 306 | d_hokui=int(hokui/100); |
makakensanba | 0:b021d725d528 | 307 | m_hokui=(hokui-d_hokui*100)/60; |
makakensanba | 0:b021d725d528 | 308 | g_hokui=d_hokui+m_hokui; |
makakensanba | 0:b021d725d528 | 309 | |
makakensanba | 0:b021d725d528 | 310 | dy = (L_HOKUI-g_hokui)/180*M_PI; |
makakensanba | 0:b021d725d528 | 311 | dx = (L_TOUKEI-g_tokei)/180*M_PI; |
makakensanba | 0:b021d725d528 | 312 | yAve = (g_hokui+L_HOKUI)/2/180*M_PI; |
makakensanba | 0:b021d725d528 | 313 | w = sqrt(1-ECC2*sin(yAve)*sin(yAve)); |
makakensanba | 0:b021d725d528 | 314 | m = A_RADIUS*(1-ECC2)/(w*w*w); |
makakensanba | 0:b021d725d528 | 315 | n = A_RADIUS/w; |
makakensanba | 0:b021d725d528 | 316 | dist = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve))/1); |
makakensanba | 0:b021d725d528 | 317 | xDist = dx*n*cos(yAve); |
makakensanba | 0:b021d725d528 | 318 | yDist = dy*m; |
makakensanba | 0:b021d725d528 | 319 | |
makakensanba | 0:b021d725d528 | 320 | float alt = ms5607.getAltitude(); |
makakensanba | 0:b021d725d528 | 321 | //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head); |
makakensanba | 0:b021d725d528 | 322 | device.printf("Radio \r\n"); |
makakensanba | 0:b021d725d528 | 323 | |
makakensanba | 0:b021d725d528 | 324 | /* |
makakensanba | 0:b021d725d528 | 325 | static int k=0; |
makakensanba | 0:b021d725d528 | 326 | if (k++ % 6 == 0){ |
makakensanba | 0:b021d725d528 | 327 | float alt = ms5607.getAltitude(); |
makakensanba | 0:b021d725d528 | 328 | //device.printf("Lon:%.6f, Lat:%.6f,vel_norm:%f,vel_head:%f\r\n",g_tokei, g_hokui,vel_norm,vel_head); |
makakensanba | 0:b021d725d528 | 329 | device.printf("Radio \r\n"); |
makakensanba | 0:b021d725d528 | 330 | |
makakensanba | 0:b021d725d528 | 331 | twe.printf("%c,%f %f %f %f %f %d \r\n",Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u); |
makakensanba | 0:b021d725d528 | 332 | fprintf(fp,"%c,%f %f %f %f %f %d \r\n" ,Phase,hokui,tokei,vel_norm,vel_head,alt,alt_launch,u); |
makakensanba | 0:b021d725d528 | 333 | |
makakensanba | 0:b021d725d528 | 334 | if(k % 30 == 0){ |
makakensanba | 0:b021d725d528 | 335 | k = 0; |
makakensanba | 0:b021d725d528 | 336 | row =! row; |
makakensanba | 0:b021d725d528 | 337 | fclose(fp); |
makakensanba | 0:b021d725d528 | 338 | fp = fopen("/sd/log.txt", "a"); |
makakensanba | 0:b021d725d528 | 339 | } |
makakensanba | 0:b021d725d528 | 340 | } |
makakensanba | 0:b021d725d528 | 341 | */ static float alt_drop2=ms5607.getAltitude(); |
makakensanba | 0:b021d725d528 | 342 | alt_drop2=ms5607.getAltitude(); |
makakensanba | 0:b021d725d528 | 343 | alt_drop=alt_drop2-alt_launch; |
makakensanba | 0:b021d725d528 | 344 | |
makakensanba | 0:b021d725d528 | 345 | float x_s[] = { xDist, yDist }; |
makakensanba | 0:b021d725d528 | 346 | device.printf("x_s %f %f drp:%f max:%f \r\n",x_s[0],x_s[1],alt_drop,alt_max - alt_launch); |
makakensanba | 0:b021d725d528 | 347 | u = set_input(st == 'A', x_s, vel_norm, vel_head, alt_drop, alt_max - alt_launch); |
makakensanba | 0:b021d725d528 | 348 | device.printf("%d",u); |
makakensanba | 0:b021d725d528 | 349 | 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); |
makakensanba | 0:b021d725d528 | 350 | |
makakensanba | 0:b021d725d528 | 351 | static int ui = 0; |
makakensanba | 0:b021d725d528 | 352 | const int ui_max = 30, ui_min = 10; // ui_min = 0 ? |
makakensanba | 0:b021d725d528 | 353 | if (ui < ui_min) u = 1; |
makakensanba | 0:b021d725d528 | 354 | else if (ui > ui_max) u = -1; |
makakensanba | 0:b021d725d528 | 355 | ui += u; |
makakensanba | 0:b021d725d528 | 356 | |
makakensanba | 0:b021d725d528 | 357 | if(u == -1) _Motor(2,FORW);//右旋回 |
makakensanba | 0:b021d725d528 | 358 | else if(u == 0) _Motor(2,STOP); |
makakensanba | 0:b021d725d528 | 359 | else if(u == 1) _Motor(2,BACK);//左旋回 |
makakensanba | 0:b021d725d528 | 360 | |
makakensanba | 0:b021d725d528 | 361 | |
makakensanba | 0:b021d725d528 | 362 | sprintf(gps_data, ""); |
makakensanba | 0:b021d725d528 | 363 | }//if |
makakensanba | 0:b021d725d528 | 364 | } |
makakensanba | 0:b021d725d528 | 365 | } |
makakensanba | 0:b021d725d528 | 366 | } |
makakensanba | 0:b021d725d528 | 367 | |
makakensanba | 0:b021d725d528 | 368 | void _Input(){ |
makakensanba | 0:b021d725d528 | 369 | input_buff[input_cnt] = device.getc(); |
makakensanba | 0:b021d725d528 | 370 | device.printf("\r\n"); |
makakensanba | 0:b021d725d528 | 371 | switch (Mode) { |
makakensanba | 0:b021d725d528 | 372 | case STANDBY: |
makakensanba | 0:b021d725d528 | 373 | if(input_cnt==0){ |
makakensanba | 0:b021d725d528 | 374 | if(input_buff[0]=='S'){ |
makakensanba | 0:b021d725d528 | 375 | device.printf("U >> UNLOCK\r\n"); |
makakensanba | 0:b021d725d528 | 376 | device.printf("L >> LOCK\r\n"); |
makakensanba | 0:b021d725d528 | 377 | }else if(input_buff[0]=='M'){ |
makakensanba | 0:b021d725d528 | 378 | device.printf("S >> STANDBY\r\n"); |
makakensanba | 0:b021d725d528 | 379 | device.printf("F >> FLIGHT\r\n"); |
makakensanba | 0:b021d725d528 | 380 | }else{ |
makakensanba | 0:b021d725d528 | 381 | device.printf("This command is not found >> %c\r\n",input_buff[0]); |
makakensanba | 0:b021d725d528 | 382 | device.printf(">>MAINMENU<<\r\n"); |
makakensanba | 0:b021d725d528 | 383 | device.printf("S >> Servo Operation\r\n"); |
makakensanba | 0:b021d725d528 | 384 | device.printf("M >> Mode Change\r\n"); |
makakensanba | 0:b021d725d528 | 385 | device.printf("-->>"); |
makakensanba | 0:b021d725d528 | 386 | return; |
makakensanba | 0:b021d725d528 | 387 | } |
makakensanba | 0:b021d725d528 | 388 | }else if(input_cnt==1){ |
makakensanba | 0:b021d725d528 | 389 | if(input_buff[0]=='S'){ |
makakensanba | 0:b021d725d528 | 390 | if(input_buff[1]=='U')_Motor(1,UNLOCK); |
makakensanba | 0:b021d725d528 | 391 | else if(input_buff[1]=='L')_Motor(1,LOCK); |
makakensanba | 0:b021d725d528 | 392 | else{ |
makakensanba | 0:b021d725d528 | 393 | device.printf("This command is not found >> %c\r\n",input_buff[1]); |
makakensanba | 0:b021d725d528 | 394 | device.printf("U >> UNLOCK\r\n"); |
makakensanba | 0:b021d725d528 | 395 | device.printf("L >> LOCK\r\n"); |
makakensanba | 0:b021d725d528 | 396 | device.printf("-->>"); |
makakensanba | 0:b021d725d528 | 397 | return; |
makakensanba | 0:b021d725d528 | 398 | } |
makakensanba | 0:b021d725d528 | 399 | }else if(input_buff[0]=='M'){ |
makakensanba | 0:b021d725d528 | 400 | if(input_buff[1]=='S'){ |
makakensanba | 0:b021d725d528 | 401 | Mode = STANDBY; |
makakensanba | 0:b021d725d528 | 402 | }else if(input_buff[1]=='F'){ |
makakensanba | 0:b021d725d528 | 403 | Mode = FLIGHT; |
makakensanba | 0:b021d725d528 | 404 | Phase = LAUNCH; |
makakensanba | 0:b021d725d528 | 405 | |
makakensanba | 0:b021d725d528 | 406 | device.printf("FLIGHT-Mode ON!\r\n"); |
makakensanba | 0:b021d725d528 | 407 | device.printf("***alert***\r\n"); |
makakensanba | 0:b021d725d528 | 408 | device.printf("You will be able to reset only!\r\n"); |
makakensanba | 0:b021d725d528 | 409 | return; |
makakensanba | 0:b021d725d528 | 410 | }else{ |
makakensanba | 0:b021d725d528 | 411 | device.printf("This command is not found >> %c\r\n",input_buff[1]); |
makakensanba | 0:b021d725d528 | 412 | device.printf("S >> STANDBY\r\n"); |
makakensanba | 0:b021d725d528 | 413 | device.printf("F >> FLIGHT\r\n"); |
makakensanba | 0:b021d725d528 | 414 | device.printf("-->>"); |
makakensanba | 0:b021d725d528 | 415 | return; |
makakensanba | 0:b021d725d528 | 416 | } |
makakensanba | 0:b021d725d528 | 417 | } |
makakensanba | 0:b021d725d528 | 418 | input_cnt = 0; |
makakensanba | 0:b021d725d528 | 419 | device.printf(">>MAINMENU<<\r\n"); |
makakensanba | 0:b021d725d528 | 420 | device.printf("S >> Servo Operation\r\n"); |
makakensanba | 0:b021d725d528 | 421 | device.printf("M >> Mode Change\r\n"); |
makakensanba | 0:b021d725d528 | 422 | device.printf("-->>"); |
makakensanba | 0:b021d725d528 | 423 | return; |
makakensanba | 0:b021d725d528 | 424 | } |
makakensanba | 0:b021d725d528 | 425 | device.printf("-->>"); |
makakensanba | 0:b021d725d528 | 426 | input_cnt++; |
makakensanba | 0:b021d725d528 | 427 | break; |
makakensanba | 0:b021d725d528 | 428 | case FLIGHT://reset only |
makakensanba | 0:b021d725d528 | 429 | break; |
makakensanba | 0:b021d725d528 | 430 | } |
makakensanba | 0:b021d725d528 | 431 | } |
makakensanba | 0:b021d725d528 | 432 | |
makakensanba | 0:b021d725d528 | 433 | /*その他雑関数*/ |
makakensanba | 0:b021d725d528 | 434 | float _Measure_Alt(float press/*[hPa]*/, float temp/*[℃]*/){ |
makakensanba | 0:b021d725d528 | 435 | return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; |
makakensanba | 0:b021d725d528 | 436 | } |
makakensanba | 0:b021d725d528 | 437 | float _Median(float data[], int num){ |
makakensanba | 0:b021d725d528 | 438 | float median; |
makakensanba | 0:b021d725d528 | 439 | float *sort = (float *)malloc(sizeof(float)*num); |
makakensanba | 0:b021d725d528 | 440 | for(int i=0; i<num; i++) sort[i] = data[i]; |
makakensanba | 0:b021d725d528 | 441 | for(int i=0; i<num; i++){ |
makakensanba | 0:b021d725d528 | 442 | for(int j=0; j<num-i-1; j++){ |
makakensanba | 0:b021d725d528 | 443 | if(sort[j]>sort[j+1]){ |
makakensanba | 0:b021d725d528 | 444 | float buff = sort[j+1]; |
makakensanba | 0:b021d725d528 | 445 | sort[j+1] = sort[j]; |
makakensanba | 0:b021d725d528 | 446 | sort[j] = buff; |
makakensanba | 0:b021d725d528 | 447 | } |
makakensanba | 0:b021d725d528 | 448 | } |
makakensanba | 0:b021d725d528 | 449 | } |
makakensanba | 0:b021d725d528 | 450 | if(num%2!=0)median = sort[num/2]; |
makakensanba | 0:b021d725d528 | 451 | else median = (sort[num/2-1]+sort[num/2])/2.0; |
makakensanba | 0:b021d725d528 | 452 | free(sort); |
makakensanba | 0:b021d725d528 | 453 | return median; |
makakensanba | 0:b021d725d528 | 454 | } |
makakensanba | 0:b021d725d528 | 455 | |
makakensanba | 0:b021d725d528 | 456 | int set_input(bool gps_good, float x_s[], float vg_knot, float hdg_gps, float alt_s, float alt_max) |
makakensanba | 0:b021d725d528 | 457 | { |
makakensanba | 0:b021d725d528 | 458 | // グローバルにするかも? |
makakensanba | 0:b021d725d528 | 459 | const int dn = 40; // windstarのベクトルを取得するステップ間隔 |
makakensanba | 0:b021d725d528 | 460 | const float vz_est = 4.62; // パラフォイルのsinkrate[m/s] |
makakensanba | 0:b021d725d528 | 461 | const float dist_tgt = 30.0; // 目標旋回半径[m] |
makakensanba | 0:b021d725d528 | 462 | const float alt_unctrl = 50.0; // 無制御で降下する高度[m] |
makakensanba | 0:b021d725d528 | 463 | |
makakensanba | 0:b021d725d528 | 464 | static int gps_step = 1; |
makakensanba | 0:b021d725d528 | 465 | float u = 0.0; |
makakensanba | 0:b021d725d528 | 466 | if (gps_good) |
makakensanba | 0:b021d725d528 | 467 | { |
makakensanba | 0:b021d725d528 | 468 | enum Phase |
makakensanba | 0:b021d725d528 | 469 | { |
makakensanba | 0:b021d725d528 | 470 | WIND_EST, |
makakensanba | 0:b021d725d528 | 471 | CTRL |
makakensanba | 0:b021d725d528 | 472 | }; |
makakensanba | 0:b021d725d528 | 473 | static Phase phase = WIND_EST; |
makakensanba | 0:b021d725d528 | 474 | |
makakensanba | 0:b021d725d528 | 475 | // sensor data |
makakensanba | 0:b021d725d528 | 476 | float dt_ = dt*gps_step; |
makakensanba | 0:b021d725d528 | 477 | gps_step = 1; |
makakensanba | 0:b021d725d528 | 478 | |
makakensanba | 0:b021d725d528 | 479 | // state variable estimation |
makakensanba | 0:b021d725d528 | 480 | float hdg_s = hdg_conv(hdg_gps); |
makakensanba | 0:b021d725d528 | 481 | static float hdg0 = hdg_s; |
makakensanba | 0:b021d725d528 | 482 | float omg_calc = hdg_s - hdg0; |
makakensanba | 0:b021d725d528 | 483 | if (omg_calc >= M_PI) omg_calc -= 2 * M_PI; |
makakensanba | 0:b021d725d528 | 484 | else if (omg_calc <= -M_PI) omg_calc += 2 * M_PI; |
makakensanba | 0:b021d725d528 | 485 | omg_calc /= dt_; |
makakensanba | 0:b021d725d528 | 486 | hdg0 = hdg_s; |
makakensanba | 0:b021d725d528 | 487 | |
makakensanba | 0:b021d725d528 | 488 | // static float alt0 = alt_s; |
makakensanba | 0:b021d725d528 | 489 | // float vz_calc = (alt_s - alt0) / dt_; |
makakensanba | 0:b021d725d528 | 490 | // alt0 = alt_s; |
makakensanba | 0:b021d725d528 | 491 | |
makakensanba | 0:b021d725d528 | 492 | float vg_s[] = { vg_knot*0.514444444*cos(hdg_s), vg_knot*0.514444444*sin(hdg_s) }; |
makakensanba | 0:b021d725d528 | 493 | // if want to use estimated position, uncomment below |
makakensanba | 0:b021d725d528 | 494 | // float x_est[2] = {}; |
makakensanba | 0:b021d725d528 | 495 | // filter_x(dt_, 0.1, x_est, x_s, vg_s); |
makakensanba | 0:b021d725d528 | 496 | |
makakensanba | 0:b021d725d528 | 497 | // wind estimation sequence |
makakensanba | 0:b021d725d528 | 498 | static float vw_est[2] = {}; |
makakensanba | 0:b021d725d528 | 499 | switch (phase) |
makakensanba | 0:b021d725d528 | 500 | { |
makakensanba | 0:b021d725d528 | 501 | case WIND_EST: |
makakensanba | 0:b021d725d528 | 502 | { |
makakensanba | 0:b021d725d528 | 503 | const int dn2 = 2 * dn; |
makakensanba | 0:b021d725d528 | 504 | static int n = 0; |
makakensanba | 0:b021d725d528 | 505 | float vw_t[2] = {}; |
makakensanba | 0:b021d725d528 | 506 | static int cnt = 0; |
makakensanba | 0:b021d725d528 | 507 | if (n >= dn2) |
makakensanba | 0:b021d725d528 | 508 | { |
makakensanba | 0:b021d725d528 | 509 | static int j = 0; |
makakensanba | 0:b021d725d528 | 510 | static float vg_buf[dn2][2] = {}; |
makakensanba | 0:b021d725d528 | 511 | windstar(vw_t, vg_buf[n % dn2], vg_buf[(n + dn) % dn2], vg_s); |
makakensanba | 0:b021d725d528 | 512 | vw_est[0] = (vw_est[0] * j + vw_t[0]) / (j + 1); |
makakensanba | 0:b021d725d528 | 513 | vw_est[1] = (vw_est[1] * j + vw_t[1]) / (j + 1); |
makakensanba | 0:b021d725d528 | 514 | j++; |
makakensanba | 0:b021d725d528 | 515 | vg_buf[n % dn2][0] = vg_s[0]; |
makakensanba | 0:b021d725d528 | 516 | vg_buf[n % dn2][1] = vg_s[1]; |
makakensanba | 0:b021d725d528 | 517 | } |
makakensanba | 0:b021d725d528 | 518 | n++; |
makakensanba | 0:b021d725d528 | 519 | |
makakensanba | 0:b021d725d528 | 520 | if (alt_s < alt_max - alt_unctrl) // |
makakensanba | 0:b021d725d528 | 521 | if (cnt++ >= 3) phase = CTRL; |
makakensanba | 0:b021d725d528 | 522 | } |
makakensanba | 0:b021d725d528 | 523 | break; |
makakensanba | 0:b021d725d528 | 524 | case CTRL: |
makakensanba | 0:b021d725d528 | 525 | { |
makakensanba | 0:b021d725d528 | 526 | float t_flt_est = (alt_s - x_tgt[2]) / vz_est; |
makakensanba | 0:b021d725d528 | 527 | if (abs(vz_est) <= epsilon) t_flt_est = 0.0; |
makakensanba | 0:b021d725d528 | 528 | float windoffset[2] = { -vw_est[0] * t_flt_est, -vw_est[1] * t_flt_est }; |
makakensanba | 0:b021d725d528 | 529 | if (windoffset[1] > 200.0) windoffset[1] = 200.0; |
makakensanba | 0:b021d725d528 | 530 | float x_virt[2] = { x_s[0] + windoffset[0], x_s[1] + windoffset[1] }; |
makakensanba | 0:b021d725d528 | 531 | // if want to use estimated position, comment out above and uncomment below |
makakensanba | 0:b021d725d528 | 532 | // float x_virt[2] = { x_est[0] + windoffset[0], x_est[1] + windoffset[1] }; |
makakensanba | 0:b021d725d528 | 533 | float x_virt_rel[2] = { x_virt[0] - x_tgt[0], x_virt[1] - x_tgt[1] }; |
makakensanba | 0:b021d725d528 | 534 | u = control(x_virt_rel, vw_est, vg_s, hdg_s, omg_calc, dist_tgt); |
makakensanba | 0:b021d725d528 | 535 | } |
makakensanba | 0:b021d725d528 | 536 | break; |
makakensanba | 0:b021d725d528 | 537 | } |
makakensanba | 0:b021d725d528 | 538 | |
makakensanba | 0:b021d725d528 | 539 | } |
makakensanba | 0:b021d725d528 | 540 | else |
makakensanba | 0:b021d725d528 | 541 | gps_step++; |
makakensanba | 0:b021d725d528 | 542 | |
makakensanba | 0:b021d725d528 | 543 | return u; |
makakensanba | 0:b021d725d528 | 544 | } |
makakensanba | 0:b021d725d528 | 545 | |
makakensanba | 0:b021d725d528 | 546 | int control(float r_rel[], float vw[], float v[], float hdg, float omg, float dist_tgt) |
makakensanba | 0:b021d725d528 | 547 | { |
makakensanba | 0:b021d725d528 | 548 | const float threshold = 0.20; |
makakensanba | 0:b021d725d528 | 549 | |
makakensanba | 0:b021d725d528 | 550 | float theta = atan2(r_rel[1], r_rel[0]); |
makakensanba | 0:b021d725d528 | 551 | |
makakensanba | 0:b021d725d528 | 552 | float dist = norm2(r_rel); |
makakensanba | 0:b021d725d528 | 553 | float Vr = dot2(r_rel, v) / dist; |
makakensanba | 0:b021d725d528 | 554 | float Vtheta = (r_rel[0] * v[1] - r_rel[1] * v[0]) / dist; |
makakensanba | 0:b021d725d528 | 555 | |
makakensanba | 0:b021d725d528 | 556 | float hdg_tgt = 0.0; |
makakensanba | 0:b021d725d528 | 557 | float alpha = atan(0.5*(dist - dist_tgt)); |
makakensanba | 0:b021d725d528 | 558 | float limitalpha = M_PI_2 - asin(dist_tgt / dist); |
makakensanba | 0:b021d725d528 | 559 | if (alpha > limitalpha) alpha = limitalpha; |
makakensanba | 0:b021d725d528 | 560 | if (Vtheta > 0) |
makakensanba | 0:b021d725d528 | 561 | hdg_tgt = theta + M_PI_2 + alpha; |
makakensanba | 0:b021d725d528 | 562 | else |
makakensanba | 0:b021d725d528 | 563 | hdg_tgt = theta - M_PI_2 - alpha; |
makakensanba | 0:b021d725d528 | 564 | periodize(hdg_tgt, M_PI, -M_PI); |
makakensanba | 0:b021d725d528 | 565 | |
makakensanba | 0:b021d725d528 | 566 | float dhdg = hdg - hdg_tgt; |
makakensanba | 0:b021d725d528 | 567 | periodize(dhdg, M_PI, -M_PI); |
makakensanba | 0:b021d725d528 | 568 | |
makakensanba | 0:b021d725d528 | 569 | float domg = omg - Vtheta / dist; |
makakensanba | 0:b021d725d528 | 570 | float Kp = 0.5, Kd = 3.0; |
makakensanba | 0:b021d725d528 | 571 | float u = -Kp*dhdg - Kd*domg; |
makakensanba | 0:b021d725d528 | 572 | |
makakensanba | 0:b021d725d528 | 573 | if (dist < 200.0) |
makakensanba | 0:b021d725d528 | 574 | { |
makakensanba | 0:b021d725d528 | 575 | if (Vtheta > 0) u += 0.05*Vr; |
makakensanba | 0:b021d725d528 | 576 | else u -= 0.05*Vr; |
makakensanba | 0:b021d725d528 | 577 | } |
makakensanba | 0:b021d725d528 | 578 | |
makakensanba | 0:b021d725d528 | 579 | if (u > threshold) return 1; |
makakensanba | 0:b021d725d528 | 580 | else if (u < -threshold) return -1; |
makakensanba | 0:b021d725d528 | 581 | else return 0; |
makakensanba | 0:b021d725d528 | 582 | } |
makakensanba | 0:b021d725d528 | 583 | |
makakensanba | 0:b021d725d528 | 584 | void filter_x(float dt, float C, float x_est[], float x_gps[], float vg_gps[]) |
makakensanba | 0:b021d725d528 | 585 | { |
makakensanba | 0:b021d725d528 | 586 | static float x0[] = { x_gps[0], x_gps[1], x_gps[2] }; |
makakensanba | 0:b021d725d528 | 587 | static int called = 0; |
makakensanba | 0:b021d725d528 | 588 | if (!called) |
makakensanba | 0:b021d725d528 | 589 | { |
makakensanba | 0:b021d725d528 | 590 | for (int i = 0; i < 2; i++) |
makakensanba | 0:b021d725d528 | 591 | x_est[i] = x0[i]; |
makakensanba | 0:b021d725d528 | 592 | called++; |
makakensanba | 0:b021d725d528 | 593 | return; |
makakensanba | 0:b021d725d528 | 594 | } |
makakensanba | 0:b021d725d528 | 595 | float xt[2]; |
makakensanba | 0:b021d725d528 | 596 | for (int i = 0; i < 2; i++) |
makakensanba | 0:b021d725d528 | 597 | { |
makakensanba | 0:b021d725d528 | 598 | xt[i] = x0[i] + vg_gps[i] * dt; |
makakensanba | 0:b021d725d528 | 599 | x_est[i] = C*x_gps[i] + (1 - C)*xt[i]; |
makakensanba | 0:b021d725d528 | 600 | x0[i] = x_est[i]; |
makakensanba | 0:b021d725d528 | 601 | } |
makakensanba | 0:b021d725d528 | 602 | } |
makakensanba | 0:b021d725d528 | 603 | |
makakensanba | 0:b021d725d528 | 604 | float hdg_conv(float hdgN0deg) |
makakensanba | 0:b021d725d528 | 605 | { |
makakensanba | 0:b021d725d528 | 606 | hdgN0deg *= -M_PI / 180.0; |
makakensanba | 0:b021d725d528 | 607 | hdgN0deg += M_PI_2; |
makakensanba | 0:b021d725d528 | 608 | periodize(hdgN0deg, M_PI, -M_PI); |
makakensanba | 0:b021d725d528 | 609 | return hdgN0deg; |
makakensanba | 0:b021d725d528 | 610 | } |
makakensanba | 0:b021d725d528 | 611 | void periodize(float &x, float max, float min) |
makakensanba | 0:b021d725d528 | 612 | { |
makakensanba | 0:b021d725d528 | 613 | float range = max - min; |
makakensanba | 0:b021d725d528 | 614 | while (x > max) x -= range; |
makakensanba | 0:b021d725d528 | 615 | while (x < min) x += range; |
makakensanba | 0:b021d725d528 | 616 | } |
makakensanba | 0:b021d725d528 | 617 | |
makakensanba | 0:b021d725d528 | 618 | void windstar(float Vw[], float Vg0[], float Vg1[], float Vg2[]) |
makakensanba | 0:b021d725d528 | 619 | { |
makakensanba | 0:b021d725d528 | 620 | float l[3][3] = {}; // line coeffs l[0]*x + l[1]*y + l[2] = 0 |
makakensanba | 0:b021d725d528 | 621 | segmentBisector(l[0], Vg0, Vg1); |
makakensanba | 0:b021d725d528 | 622 | segmentBisector(l[1], Vg1, Vg2); |
makakensanba | 0:b021d725d528 | 623 | segmentBisector(l[2], Vg2, Vg0); |
makakensanba | 0:b021d725d528 | 624 | float r[3][3] = {}; // intersection point |
makakensanba | 0:b021d725d528 | 625 | intersection(r[0], l[0], l[1]); |
makakensanba | 0:b021d725d528 | 626 | intersection(r[1], l[1], l[2]); |
makakensanba | 0:b021d725d528 | 627 | intersection(r[2], l[2], l[0]); |
makakensanba | 0:b021d725d528 | 628 | Vw[0] = (r[0][0] + r[1][0] + r[2][0]) / 3; |
makakensanba | 0:b021d725d528 | 629 | Vw[1] = (r[0][1] + r[1][1] + r[2][1]) / 3; |
makakensanba | 0:b021d725d528 | 630 | } |
makakensanba | 0:b021d725d528 | 631 | void segmentBisector(float a[], float v1[], float v2[]) |
makakensanba | 0:b021d725d528 | 632 | { |
makakensanba | 0:b021d725d528 | 633 | a[0] = v2[0] - v1[0]; |
makakensanba | 0:b021d725d528 | 634 | a[1] = v2[1] - v1[1]; |
makakensanba | 0:b021d725d528 | 635 | a[2] = -0.5*(a[0] * (v1[0] + v2[0]) + a[1] * (v1[1] + v2[1])); |
makakensanba | 0:b021d725d528 | 636 | } |
makakensanba | 0:b021d725d528 | 637 | int intersection(float r[], float l[], float m[]) |
makakensanba | 0:b021d725d528 | 638 | { |
makakensanba | 0:b021d725d528 | 639 | float det = l[0] * m[1] - l[1] * m[0]; |
makakensanba | 0:b021d725d528 | 640 | if (abs(det) < epsilon) return -1; |
makakensanba | 0:b021d725d528 | 641 | r[0] = (-m[1] * l[2] + l[1] * m[2]) / det; |
makakensanba | 0:b021d725d528 | 642 | r[1] = (m[0] * l[2] - l[0] * m[2]) / det; |
makakensanba | 0:b021d725d528 | 643 | return 0; |
makakensanba | 0:b021d725d528 | 644 | } |
makakensanba | 0:b021d725d528 | 645 | |
makakensanba | 0:b021d725d528 | 646 | float dot2(float a[], float b[]) |
makakensanba | 0:b021d725d528 | 647 | { |
makakensanba | 0:b021d725d528 | 648 | return a[0] * b[0] + a[1] * b[1]; |
makakensanba | 0:b021d725d528 | 649 | } |
makakensanba | 0:b021d725d528 | 650 | float norm2(float v[]) |
makakensanba | 0:b021d725d528 | 651 | { |
makakensanba | 0:b021d725d528 | 652 | return sqrt(dot2(v, v)); |
makakensanba | 0:b021d725d528 | 653 | } |