ヌペリオン星人の偵察型UAV「ヴェネサス」.その飛行アルゴリズムを解析し,C言語にて再現したもの.
Dependencies: MPU6050_2 MS5607 SDFileSystem mbed
Fork of 00NUPERION_ver12 by
main.cpp
- Committer:
- naoki_westwell
- Date:
- 2017-03-16
- Revision:
- 0:98c7c6fe50cc
- Child:
- 1:491e67888f28
File content as of revision 0:98c7c6fe50cc:
#include "mbed.h" #include "MS5607I2C.h" #include "SDFileSystem.h" #include "MPU6050.h" #include <stdio.h> /***************************/ /* 打上前に値を確認!!!!! */ /***************************/ #define L_HOKUI 35.546257 // 10進法 KBIC 3本の樹 #define L_TOUKEI 139.671875 #define LNCH_ACC 2 // 発射判定閾値[G] #define TRANS_TIME 50 // 打上から制御開始までの時間[sec/10] /***************************/ #define DEG_TO_RAD(x) ( x * 0.01745329 ) #define RAD_TO_DEG(x) ( x * 57.29578 ) #define HIGH 1 #define LOW 0 #define STDBY_MODE 0 // 待機 #define TRANS_MODE 1 // 輸送 #define CTRL_MODE 2 // 制御 #define GPS_RATE 10 // GPSの取得レート #define SAVE_RATE 50 // SDへの保存の間隔[sec/10] #define ACC_SSF 2048 //Sensitivity Scale Factor #define GYRO_SSF 16.4 #define PRESS_INIT 10 // 基準気圧の平均を取る数 #define KPL 2.0 // ラダーゲイン #define KPE 2.0 // エレベータゲイン #define GOAL_R 50 // 目標地点にこれだけ近づいたら制御方法変える #define DELTA_E_FULLUP 2000 #define DELTA_E_NTRL 1500 #define DELTA_E_FULLDOWN 1000 #define DELTA_L_FULLR 2000 #define DELTA_L_NTRL 1500 #define DELTA_L_FULLL 1000 #define PI 3.14159265358979 // π #define A_RADIUS 6378137.000 // 地球長半径 [m] #define B_RADIUS 6356752.314245 // 地球短半径 (WGS84)[m] #define ECC2 0.006694379990 // 離心率の二乗 /* ペリフェラル宣言 */ Serial pc(USBTX,USBRX); Serial gps(p9, p10); Serial twe(p13, p14); PwmOut eServo(p21); // ラダー用サーボ PwmOut lServo(p22); // エレベータ用サーボ DigitalOut camSw(p20); // カメラスイッチ SDFileSystem sd(p5, p6, p7, p8, "sd"); MPU6050 mpu; MS5607I2C ms5607(p28, p27, true); BusOut leds(LED1, LED2, LED3, LED4); /* グローバル変数宣言 */ int mode = STDBY_MODE; int cnt = 0; int gps_i; // 文字数,モード(センテンス終わりか?判定) char gps_data[256]; float beta, alpha; float set_point_l, set_point_e; float delta_l=1500, delta_e=1500; FILE *fp; char all_data[256]; /* プロトタイプ宣言 */ bool Init(); void getData(); float getAlt(float press, float press0, float temp); //void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[]); void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz); void filter_vz(double dt, double C, double &vz_est, double vz); bool Init() { gps.baud(9600); // ボーレート pc.baud(115200); twe.baud(115200); eServo.period_us(20000); // servo requires a 20ms period lServo.period_us(20000); lServo.pulsewidth_us(0); eServo.pulsewidth_us(0); 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だけにする gps.printf("$PMTK300,100,0,0,0,0*2C\r\n"); // position fixを100msecに(レート10spsにする) mpu.initialize(); ms5607.printCoefficients(); pc.puts("Init finish!\n"); return true; } /******************/ // GPSの取得周期(10SPS)に沿って,他のデータ取得等の処理を行う. /******************/ void getData() { char c = gps.getc(); if( c=='$' || gps_i == 256) { gps_i = 0; } if((gps_data[gps_i]=c) != '\r') { gps_i++; } else { gps_data[gps_i]='\0'; float w_time, hokui, tokei; // 時刻,北緯,東経 float vb, beta2; // 速度,真方位 char ns, ew, status; // NS,EW,その他 if( sscanf(gps_data, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,",&w_time,&status,&hokui,&ns,&tokei,&ew,&vb,&beta2) >= 1) { //Longitude double d_tokei= int(tokei/100); double m_tokei= (tokei-d_tokei*100)/60; double g_tokei= d_tokei+m_tokei; // 10進法に換算 //Latitude double d_hokui= int(hokui/100); double m_hokui= (hokui-d_hokui*100)/60; double g_hokui= d_hokui+m_hokui; /* ヒュベニの公式 */ double dy = (L_HOKUI-g_hokui)/180*PI; //ラジアン double dx = (L_TOUKEI-g_tokei)/180*PI; double yAve = (g_hokui+L_HOKUI)/2/180*PI; double w = sqrt(1-ECC2*sin(yAve)*sin(yAve)); double m = A_RADIUS*(1-ECC2)/(w*w*w); double n = A_RADIUS/w; double distance = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve))); double xDist = dx*n*cos(yAve); double yDist = dy*m; /* 高度 */ static float alt, alt0; //前の高度 static float press_arr[PRESS_INIT]; // 基準高度はこれの平均をとる static float press0 = 0; float temp = ms5607.getTemperature(); // 気圧 float press = ms5607.getPressure(); // 温度 /* 基準電圧の取得 */ if(mode == STDBY_MODE) { for(int i=0; i<PRESS_INIT-1; i++) press_arr[i] = press_arr[i+1]; // 気圧データをシフト press_arr[PRESS_INIT-1] = press; } if(mode == TRANS_MODE) { //発射直後,基準電圧を計算 if(cnt==0 && press0==0) { for(int i=0; i<PRESS_INIT; i++) press0 += press_arr[i]; press0 /= PRESS_INIT; } } double vz_calc = (alt - alt0) / 0.1; alt0 = alt; alt = getAlt(press, press0, temp); //高度の算出 double alt_est = 0.0, vz_est = 0.0; float dt = 1/GPS_RATE; filter_alt(dt, 0.1, alt_est, alt, vz_est); filter_vz(dt, 0.01, vz_est, vz_calc); /* 加速度 */ int16_t ax, ay, az; // mpu生データ int16_t gx, gy, gz; mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); float fax = float(ax)/ACC_SSF; // mpu物理量に換算 float fay = float(ay)/ACC_SSF; float faz = float(az)/ACC_SSF; //float fgx = float(gx)/GYRO_SSF; //float fgy =float(gy)/GYRO_SSF; //float fgz = float(gz)/GYRO_SSF; float resAcc = sqrt(fax*fax + fay*fay + faz*faz); //合成加速度 /* アングルオフ */ if(beta2 > 180) beta2 -= 360; double beta3 = RAD_TO_DEG(atan2(yDist, xDist)); beta = beta2 - beta3; if(beta > 180) beta -= 360; if(beta <-180) beta += 360; /* 降下角 */ alpha = -atan(vz_est/vb); /* サーボ操作 P制御 */ if(distance <= GOAL_R){ // ラダー操舵 set_point_l = DELTA_L_NTRL + 500*(beta * KPL / 180); if(set_point_l > DELTA_L_FULLR) set_point_l = DELTA_L_FULLR; if(set_point_l < DELTA_L_FULLL) set_point_l = DELTA_L_FULLL; if((set_point_l-delta_l)>=0){ delta_l += 100; if((set_point_l-delta_l)<0) delta_l = set_point_l; }else{ delta_l -= 100; if((set_point_l-delta_l)>0) delta_l = set_point_l+1; } // エレベータ制御 set_point_e = DELTA_E_NTRL + 500*(alpha * KPE / 180); if(set_point_e > DELTA_E_FULLUP) set_point_e = DELTA_E_FULLUP; if(set_point_e < DELTA_E_FULLDOWN) set_point_e = DELTA_E_FULLDOWN; if((set_point_e-delta_e)>=0){ delta_e += 100; if((set_point_e-delta_e)<0) delta_e = set_point_e; }else{ delta_e -= 100; if((set_point_e-delta_e)>0) delta_e = set_point_e+1; } }else{ if(cnt%10) delta_e = DELTA_E_NTRL; else delta_e = DELTA_E_FULLUP; delta_l = DELTA_L_FULLR; // 要検討 } lServo.pulsewidth_us(delta_l); eServo.pulsewidth_us(delta_e); /* 取得データの整理と出力 */ sprintf(all_data, "%6lf,%.2f,%.2f,%.2f,%d,%d\r\n", distance, vb, beta, alt, cnt, mode); pc.printf("%s", all_data); if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d\r\n", g_tokei, g_hokui, alt, cnt); //pc.printf("%s\r\n",gps_data); //pc.printf("%f,%f\r\n", delta_l, set_point_l); /* カメラスイッチ操作 */ if(mode==TRANS_MODE){ if(cnt<=20) camSw = HIGH; // 2秒間長押しで電源オンの後,一回短押しで撮影開始 if(cnt>20 && cnt<=25) camSw = LOW; if(cnt>25 && cnt<=30) camSw = HIGH; if(cnt>30) camSw = LOW; } /* SDカードに保存 */ if((cnt%SAVE_RATE)==0) fp = fopen("/sd/myfile.txt", "a"); fprintf(fp, "%s", all_data); if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp); sprintf(gps_data, ""); /* カウント */ cnt++; if(cnt>=600) cnt = 0; /* モード遷移判定 */ if(mode == STDBY_MODE) { // 待機⇒制御 if(resAcc > LNCH_ACC) { mode = TRANS_MODE; cnt = 0; } leds = 1 << cnt%4; }else if(mode == TRANS_MODE) { if(cnt > TRANS_TIME) { mode = CTRL_MODE; cnt = 0; } leds = 3 << cnt%4; }else if(mode == CTRL_MODE) { leds = 15*(cnt%2); } } } } int main() { Init(); pc.printf("*** Start up! ***\r\n"); gps.attach(getData,Serial::RxIrq); while(1) {} } // 高度計算 float getAlt(float press, float press0, float temp) { return (pow((press0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; } // 位置(速度)フィルタ void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[]) { static double x0[] = { x_gps[0], x_gps[1], x_gps[2] }; static int called = 0; if (!called) { for (int i = 0; i < 2; i++) x_est[i] = x0[i]; called++; return; } double xt[2]; for (int i = 0; i < 2; i++) { xt[i] = x0[i] + vg_gps[i] * dt; x_est[i] = C*x_gps[i] + (1 - C)*xt[i]; x0[i] = x_est[i]; } } // 高度フィルタ void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz) { static double alt0 = alt_s; static int called = 0; if (!called) { alt_est = alt0; called++; return; } double altt = alt0 + vz * dt; { alt_est = C*alt_s + (1 - C)*altt; alt0 = alt_est; } } // 垂直速度フィルタ void filter_vz(double dt, double C, double &vz_est, double vz) { static double vz0 = 0.0; static int called = 0; if (!called) { vz_est = vz0; called++; return; } vz_est = C*vz + (1 - C)*vz0; vz0 = vz_est; }