CORE
ヌペリオン星人の偵察型UAV「ヴェネサス」．その飛行アルゴリズムを解析し，C言語にて再現したもの．
Dependencies: MPU6050_2 MS5607 SDFileSystem mbed
Fork of
00NUPERION_ver12
by 
naoki nishii
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "MS5607I2C.h" 00003 #include "SDFileSystem.h" 00004 #include "MPU6050.h" 00005 #include <stdio.h> 00006 00007 //#define TEST 00008 #define FLIGHT 00009 /***************************/ 00010 /* 打上前に値を確認！！！！！ */ 00011 /***************************/ 00012 #ifdef TEST 00013 #define L_HOKUI 34.74102777 // 10進法 urasabaku 00014 #define L_TOUKEI 139.42511111 00015 #define LNCH_ACC 2 // 発射判定閾値[G] 00016 #define TRANS_TIME 50 // 打上から制御開始までの時間[sec/10] 00017 #define OPEN_TIME 50 // 150mに達してから開放されるまで（含ロケットアビオとのラグ） 00018 #endif 00019 00020 #ifdef FLIGHT 00021 #define L_HOKUI 34.74102777 // 10進法 urasabaku 00022 #define L_TOUKEI 139.42511111 00023 #define LNCH_ACC 4 // 発射判定閾値[G] 00024 #define TRANS_TIME 100 // 打上から制御開始までの時間[sec/10] 00025 #define OPEN_TIME 50 // 150mに達してから開放されるまで（含ロケットアビオとのラグ） 00026 #endif 00027 /***************************/ 00028 00029 #define DEG_TO_RAD(x) ( x * 0.01745329 ) 00030 #define RAD_TO_DEG(x) ( x * 57.29578 ) 00031 00032 #define HIGH 1 00033 #define LOW 0 00034 00035 #define STDBY_MODE 0 // 待機 00036 #define TRANS_MODE 1 // 輸送 00037 #define CTRL_MODE 2 // 制御 00038 00039 #define GPS_RATE 10 // GPSの取得レート 00040 #define SAVE_RATE 50 // SDへの保存の間隔[sec/10] 00041 00042 #define ACC_SSF 2048 //Sensitivity Scale Factor 00043 #define GYRO_SSF 16.4 00044 #define PRESS_INIT 10 // 基準気圧の平均を取る数 00045 00046 #define KPL 2.0 // ラダーゲイン 00047 #define KPE 2.0 // エレベータゲイン 00048 00049 #define GOAL_R 50 // 目標地点にこれだけ近づいたら制御方法変える 00050 #define DELTA_E_FULLUP 1000 00051 #define DELTA_E_NTRL 1500 00052 #define DELTA_E_FULLDOWN 2000 00053 #define DELTA_L_FULLR 2000 00054 #define DELTA_L_NTRL 1500 00055 #define DELTA_L_FULLL 1000 00056 00057 #define PI 3.14159265358979 // π 00058 #define A_RADIUS 6378137.000 // 地球長半径 [m] 00059 #define B_RADIUS 6356752.314245 // 地球短半径 (WGS84)[m] 00060 #define ECC2 0.006694379990 // 離心率の二乗 00061 00062 /* ペリフェラル宣言 */ 00063 Serial pc(USBTX,USBRX); 00064 Serial gps(p9, p10); 00065 Serial twe(p13, p14); 00066 PwmOut eServo(p21); // ラダー用サーボ 00067 PwmOut lServo(p22); // エレベータ用サーボ 00068 DigitalOut camSw(p20); // カメラスイッチ 00069 SDFileSystem sd(p5, p6, p7, p8, "sd"); 00070 MPU6050 mpu; 00071 MS5607I2C ms5607(p28, p27, true); 00072 BusOut leds(LED1, LED2, LED3, LED4); 00073 00074 /* グローバル変数宣言 */ 00075 int mode = STDBY_MODE; 00076 int cnt = 0; 00077 00078 int gps_i; // 文字数，モード（センテンス終わりか？判定） 00079 char gps_data[256]; 00080 float beta, alpha; 00081 float set_point_l=DELTA_L_NTRL, set_point_e=DELTA_E_NTRL; 00082 float delta_l=0, delta_e=0; 00083 00084 FILE *fp; 00085 char all_data[256]; 00086 00087 /* プロトタイプ宣言 */ 00088 bool Init(); 00089 void getData(); 00090 float getAlt(float press, float press0, float temp); 00091 //void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz); 00092 //void filter_vz(double dt, double C, double &vz_est, double vz); 00093 00094 bool Init() 00095 { 00096 /*①*/leds = leds|1; 00097 pc.printf("*** Start up! ***\r\n"); 00098 gps.baud(9600); // ボーレート 00099 pc.baud(115200); 00100 twe.baud(115200); 00101 eServo.period_us(20000); // servo requires a 20ms period 00102 lServo.period_us(20000); 00103 00104 gps.printf("$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n"); // GPRMCだけにする 00105 gps.printf("$PMTK300,100,0,0,0,0*2C\r\n"); // position fixを100msecに（レート10spsにする） 00106 00107 mpu.initialize(); 00108 ms5607.printCoefficients(); 00109 00110 /*②*/leds = leds | (1<<1); 00111 pc.printf("Test start!\r\n"); 00112 lServo.pulsewidth_us(DELTA_L_NTRL); 00113 eServo.pulsewidth_us(DELTA_E_NTRL); 00114 wait(1); 00115 lServo.pulsewidth_us(DELTA_L_FULLR); 00116 eServo.pulsewidth_us(DELTA_E_FULLUP); 00117 wait(1); 00118 lServo.pulsewidth_us(DELTA_L_FULLL); 00119 eServo.pulsewidth_us(DELTA_E_FULLDOWN); 00120 wait(1); 00121 lServo.pulsewidth_us(DELTA_L_NTRL); 00122 eServo.pulsewidth_us(DELTA_E_NTRL); 00123 wait(1); 00124 00125 /*③*/leds = leds | (1<<2); 00126 float temp, press; 00127 float fax, fay, faz; 00128 int16_t ax, ay, az; 00129 int16_t gx, gy, gz; 00130 for(int i=0; i<10; i++){ 00131 temp = ms5607.getTemperature(); // 気圧 00132 press = ms5607.getPressure(); // 温度 00133 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 00134 fax = float(ax)/ACC_SSF; // mpu物理量に換算 00135 fay = float(ay)/ACC_SSF; 00136 faz = float(az)/ACC_SSF; 00137 twe.printf("%f,%f,%f,%f,%f,%f,%f,%f", temp, press, fax, fay, faz); 00138 wait(0.1); 00139 } 00140 00141 /*④*/leds = leds | (1<<3); 00142 pc.puts("Init finish!\n"); 00143 return true; 00144 } 00145 00146 /******************/ 00147 // GPSの取得周期（10SPS）に沿って，他のデータ取得等の処理を行う． 00148 /******************/ 00149 00150 void getData() 00151 { 00152 char c = gps.getc(); 00153 if( c=='$' || gps_i == 256) { 00154 gps_i = 0; 00155 } 00156 if((gps_data[gps_i]=c) != '\r') { 00157 gps_i++; 00158 } else { 00159 00160 gps_data[gps_i]='\0'; 00161 00162 float w_time, hokui, tokei; // 時刻，北緯，東経 00163 float vb, beta2; // 速度，真方位 00164 char ns, ew, status; // NS，EW，その他 00165 if( sscanf(gps_data, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,",&w_time,&status,&hokui,&ns,&tokei,&ew,&vb,&beta2) >= 1) { 00166 //Longitude 00167 double d_tokei= int(tokei/100); 00168 double m_tokei= (tokei-d_tokei*100)/60; 00169 double g_tokei= d_tokei+m_tokei; // 10進法に換算 00170 //Latitude 00171 double d_hokui= int(hokui/100); 00172 double m_hokui= (hokui-d_hokui*100)/60; 00173 double g_hokui= d_hokui+m_hokui; 00174 /* ヒュベニの公式 */ 00175 double dy = (L_HOKUI-g_hokui)/180*PI; //ラジアン 00176 double dx = (L_TOUKEI-g_tokei)/180*PI; 00177 double yAve = (g_hokui+L_HOKUI)/2/180*PI; 00178 double w = sqrt(1-ECC2*sin(yAve)*sin(yAve)); 00179 double m = A_RADIUS*(1-ECC2)/(w*w*w); 00180 double n = A_RADIUS/w; 00181 double distance = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve))); 00182 double xDist = dx*n*cos(yAve); 00183 double yDist = dy*m; 00184 00185 /* 高度 */ 00186 static float alt, alt0; // 高度，前の高度 00187 static float press_arr[PRESS_INIT]; // 基準気圧はこれの平均をとる 00188 static float press0 = 0; // 基準気圧（地上の気圧） 00189 float vz; // 高度，沈下速度 00190 float temp = ms5607.getTemperature(); // 気圧 00191 float press = ms5607.getPressure(); // 温度 00192 00193 /* 基準気圧の取得 */ 00194 if(mode == STDBY_MODE) { 00195 for(int i=0; i<PRESS_INIT-1; i++) press_arr[i] = press_arr[i+1]; // 気圧データをシフト 00196 press_arr[PRESS_INIT-1] = press; 00197 } 00198 if(mode == TRANS_MODE) { //発射直後，基準気圧を計算 00199 if(cnt==0 && press0==0) { 00200 for(int i=0; i<PRESS_INIT; i++) press0 += press_arr[i]; 00201 press0 /= PRESS_INIT; 00202 } 00203 } 00204 00205 /* 沈下速度計算 */ 00206 static float dt = 1/GPS_RATE; 00207 alt0 = alt; 00208 alt = getAlt(press, press0, temp); 00209 vz = (alt0 - alt)*dt; 00210 00211 /* だきゅんフィルタ */ 00212 /* 00213 double vz_calc, alt_est, vz_est; 00214 double dt = 1/GPS_RATE; 00215 if(mode >= TRANS_MODE){ 00216 static double alt0 = getAlt(press, press0, temp); //高度の算出 00217 vz_calc = (alt - alt0) / dt; 00218 alt0 = alt; 00219 alt = getAlt(press, press0, temp); //高度の算出 00220 alt_est = 0.0, vz_est = 0.0; 00221 filter_alt(dt, 1, alt_est, alt, vz_est); 00222 filter_vz(dt, 1, vz_est, vz_calc); 00223 } 00224 */ 00225 00226 /* 加速度 */ 00227 int16_t ax, ay, az; // mpu生データ 00228 int16_t gx, gy, gz; 00229 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 00230 float fax = float(ax)/ACC_SSF; // mpu物理量に換算 00231 float fay = float(ay)/ACC_SSF; 00232 float faz = float(az)/ACC_SSF; 00233 //float fgx = float(gx)/GYRO_SSF; 00234 //float fgy =float(gy)/GYRO_SSF; 00235 //float fgz = float(gz)/GYRO_SSF; 00236 float resAcc = sqrt(fax*fax + fay*fay + faz*faz); //合成加速度 00237 00238 /* アングルオフ */ 00239 if(beta2 > 180) beta2 -= 360; 00240 double beta3 = RAD_TO_DEG(atan2(yDist, xDist)); 00241 beta = beta2 - beta3; 00242 if(beta > 180) beta -= 360; 00243 if(beta <-180) beta += 360; 00244 00245 /* 降下角 */ 00246 alpha = atan(vz/vb)*180/PI; 00247 00248 /* サーボ操作 P制御 */ 00249 if(mode!=CTRL_MODE) { 00250 delta_l = 0; delta_l = 0; 00251 }else if(distance > GOAL_R){ //目標地点のある半径外なら 00252 // ラダー操舵 00253 set_point_l = DELTA_L_NTRL - 500*(beta * KPL / 180); 00254 if(set_point_l > DELTA_L_FULLR) set_point_l = DELTA_L_FULLR; 00255 if(set_point_l < DELTA_L_FULLL) set_point_l = DELTA_L_FULLL; 00256 if((set_point_l-delta_l)>=0){ 00257 delta_l += 100; 00258 if((set_point_l-delta_l)<0) delta_l = set_point_l; 00259 }else{ 00260 delta_l -= 100; 00261 if((set_point_l-delta_l)>0) delta_l = set_point_l+1; 00262 } 00263 00264 // エレベータ制御 00265 if(alpha>0){ 00266 set_point_e = DELTA_E_NTRL - 500*(alpha * KPE / 180); 00267 if(set_point_e > DELTA_E_FULLUP) set_point_e = DELTA_E_FULLUP; 00268 if(set_point_e < DELTA_E_FULLDOWN) set_point_e = DELTA_E_FULLDOWN; 00269 if((set_point_e-delta_e)>=0){ 00270 delta_e += 100; 00271 if((set_point_e-delta_e)<0) delta_e = set_point_e; 00272 }else{ 00273 delta_e -= 100; 00274 if((set_point_e-delta_e)>0) delta_e = set_point_e+1; 00275 } 00276 } 00277 }else{ // ある半径内なら 00278 if(cnt%10) delta_e = DELTA_E_NTRL; 00279 else delta_e = DELTA_E_FULLUP; 00280 delta_l = DELTA_L_FULLR; // 要検討 00281 } 00282 if(g_tokei == 0){ 00283 delta_e = DELTA_E_NTRL; 00284 delta_l = DELTA_L_FULLR; 00285 } 00286 lServo.pulsewidth_us(delta_l); 00287 eServo.pulsewidth_us(delta_e); 00288 00289 /* 取得データの整理と出力 */ 00290 /*sprintf(all_data, "%6lf,%.2f,%.2f,%.2f,%d,%d\r\n", 00291 distance, vb, beta, alt, cnt, mode);*/ 00292 sprintf(all_data, "%.6f,%.6f,%.2f,%.2f,%.2f,%d,%d\r\n", 00293 g_tokei, g_hokui, alt, 00294 vb, beta2, cnt, mode); 00295 pc.printf("%s", all_data); 00296 if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d,%d\r\n", g_tokei, g_hokui, alt, cnt, mode); 00297 //pc.printf("%s\r\n",gps_data); 00298 //pc.printf("%f,%f\r\n", delta_l, set_point_l); 00299 00300 /* SDカードに保存 */ 00301 if(mode >= TRANS_MODE){ // 輸送モード以降で保存を開始 00302 if((cnt%SAVE_RATE)==0) fp = fopen("/sd/data.txt", "a"); 00303 fprintf(fp, "%s", all_data); 00304 if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp); 00305 sprintf(gps_data, ""); 00306 } 00307 00308 /* カメラスイッチ操作 */ 00309 if(mode==TRANS_MODE){ 00310 if(cnt<=20) camSw = HIGH; // 2秒間長押しで電源オンの後，一回短押しで撮影開始 00311 if(cnt>20 && cnt<=50) camSw = LOW; 00312 if(cnt>50 && cnt<=55) camSw = HIGH; 00313 if(cnt>55) camSw = LOW; 00314 }else{ 00315 camSw = LOW; 00316 } 00317 00318 /* カウント */ 00319 cnt++; 00320 if(cnt>=600) cnt = 0; 00321 00322 /* モード遷移判定 */ 00323 if(mode == STDBY_MODE) { // 待機⇒制御 00324 if(resAcc > LNCH_ACC && cnt>10) { 00325 mode = TRANS_MODE; 00326 cnt = 0; 00327 } 00328 leds = 1 << cnt%4; 00329 }else 00330 if(mode == TRANS_MODE) { 00331 if(cnt > TRANS_TIME && alt < 150) { 00332 static int ctrl_start_cnt = 0; 00333 ctrl_start_cnt++; 00334 if(ctrl_start_cnt > OPEN_TIME){ 00335 delta_l = DELTA_L_NTRL; // 制御開始時に通常のデューティ比にする 00336 delta_e = DELTA_E_NTRL; 00337 mode = CTRL_MODE; 00338 cnt = 0; 00339 } 00340 } 00341 leds = 3 << cnt%4; 00342 }else 00343 if(mode == CTRL_MODE) { 00344 leds = 15*(cnt%2); 00345 } 00346 } 00347 } 00348 } 00349 00350 int main() 00351 { 00352 Init(); 00353 gps.attach(getData,Serial::RxIrq); 00354 while(1) {} 00355 } 00356 // 高度計算 00357 float getAlt(float press, float press0, float temp) 00358 { 00359 return (pow((press0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; 00360 }
Generated on Wed Jul 13 2022 17:28:57 by
1.7.2