あああ
Dependencies: MPU6050_2 MS5607 SDFileSystem mbed
main.cpp
- Committer:
- naoki_westwell
- Date:
- 2017-04-07
- Revision:
- 1:491e67888f28
- Parent:
- 0:98c7c6fe50cc
File content as of revision 1:491e67888f28:
#include "mbed.h" #include "MS5607I2C.h" #include "SDFileSystem.h" #include "MPU6050.h" #include <stdio.h> //#define TEST #define FLIGHT /***************************/ /* 打上前に値を確認!!!!! */ /***************************/ #ifdef TEST #define L_HOKUI 34.74102777 // 10進法 urasabaku #define L_TOUKEI 139.42511111 #define LNCH_ACC 2 // 発射判定閾値[G] #define TRANS_TIME 50 // 打上から制御開始までの時間[sec/10] #define OPEN_TIME 50 // 150mに達してから開放されるまで(含ロケットアビオとのラグ) #endif #ifdef FLIGHT #define L_HOKUI 34.74102777 // 10進法 urasabaku #define L_TOUKEI 139.42511111 #define LNCH_ACC 4 // 発射判定閾値[G] #define TRANS_TIME 100 // 打上から制御開始までの時間[sec/10] #define OPEN_TIME 50 // 150mに達してから開放されるまで(含ロケットアビオとのラグ) #endif /***************************/ #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 1000 #define DELTA_E_NTRL 1500 #define DELTA_E_FULLDOWN 2000 #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=DELTA_L_NTRL, set_point_e=DELTA_E_NTRL; float delta_l=0, delta_e=0; FILE *fp; char all_data[256]; /* プロトタイプ宣言 */ bool Init(); void getData(); float getAlt(float press, float press0, float temp); //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() { /*①*/leds = leds|1; pc.printf("*** Start up! ***\r\n"); gps.baud(9600); // ボーレート pc.baud(115200); twe.baud(115200); eServo.period_us(20000); // servo requires a 20ms period lServo.period_us(20000); 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(); /*②*/leds = leds | (1<<1); pc.printf("Test start!\r\n"); lServo.pulsewidth_us(DELTA_L_NTRL); eServo.pulsewidth_us(DELTA_E_NTRL); wait(1); lServo.pulsewidth_us(DELTA_L_FULLR); eServo.pulsewidth_us(DELTA_E_FULLUP); wait(1); lServo.pulsewidth_us(DELTA_L_FULLL); eServo.pulsewidth_us(DELTA_E_FULLDOWN); wait(1); lServo.pulsewidth_us(DELTA_L_NTRL); eServo.pulsewidth_us(DELTA_E_NTRL); wait(1); /*③*/leds = leds | (1<<2); float temp, press; float fax, fay, faz; int16_t ax, ay, az; int16_t gx, gy, gz; for(int i=0; i<10; i++){ temp = ms5607.getTemperature(); // 気圧 press = ms5607.getPressure(); // 温度 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); fax = float(ax)/ACC_SSF; // mpu物理量に換算 fay = float(ay)/ACC_SSF; faz = float(az)/ACC_SSF; twe.printf("%f,%f,%f,%f,%f,%f,%f,%f", temp, press, fax, fay, faz); wait(0.1); } /*④*/leds = leds | (1<<3); 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 vz; // 高度,沈下速度 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; } } /* 沈下速度計算 */ static float dt = 1/GPS_RATE; alt0 = alt; alt = getAlt(press, press0, temp); vz = (alt0 - alt)*dt; /* だきゅんフィルタ */ /* double vz_calc, alt_est, vz_est; double dt = 1/GPS_RATE; if(mode >= TRANS_MODE){ static double alt0 = getAlt(press, press0, temp); //高度の算出 vz_calc = (alt - alt0) / dt; alt0 = alt; alt = getAlt(press, press0, temp); //高度の算出 alt_est = 0.0, vz_est = 0.0; filter_alt(dt, 1, alt_est, alt, vz_est); filter_vz(dt, 1, 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/vb)*180/PI; /* サーボ操作 P制御 */ if(mode!=CTRL_MODE) { delta_l = 0; delta_l = 0; }else 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; } // エレベータ制御 if(alpha>0){ 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; // 要検討 } if(g_tokei == 0){ delta_e = DELTA_E_NTRL; 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);*/ sprintf(all_data, "%.6f,%.6f,%.2f,%.2f,%.2f,%d,%d\r\n", g_tokei, g_hokui, alt, vb, beta2, cnt, mode); pc.printf("%s", all_data); if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d,%d\r\n", g_tokei, g_hokui, alt, cnt, mode); //pc.printf("%s\r\n",gps_data); //pc.printf("%f,%f\r\n", delta_l, set_point_l); /* SDカードに保存 */ if(mode >= TRANS_MODE){ // 輸送モード以降で保存を開始 if((cnt%SAVE_RATE)==0) fp = fopen("/sd/data.txt", "a"); fprintf(fp, "%s", all_data); if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp); sprintf(gps_data, ""); } /* カメラスイッチ操作 */ if(mode==TRANS_MODE){ if(cnt<=20) camSw = HIGH; // 2秒間長押しで電源オンの後,一回短押しで撮影開始 if(cnt>20 && cnt<=50) camSw = LOW; if(cnt>50 && cnt<=55) camSw = HIGH; if(cnt>55) camSw = LOW; }else{ camSw = LOW; } /* カウント */ cnt++; if(cnt>=600) cnt = 0; /* モード遷移判定 */ if(mode == STDBY_MODE) { // 待機⇒制御 if(resAcc > LNCH_ACC && cnt>10) { mode = TRANS_MODE; cnt = 0; } leds = 1 << cnt%4; }else if(mode == TRANS_MODE) { if(cnt > TRANS_TIME && alt < 150) { static int ctrl_start_cnt = 0; ctrl_start_cnt++; if(ctrl_start_cnt > OPEN_TIME){ delta_l = DELTA_L_NTRL; // 制御開始時に通常のデューティ比にする delta_e = DELTA_E_NTRL; mode = CTRL_MODE; cnt = 0; } } leds = 3 << cnt%4; }else if(mode == CTRL_MODE) { leds = 15*(cnt%2); } } } } int main() { Init(); 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; }