あああ
Dependencies: MPU6050_2 MS5607 SDFileSystem mbed
Diff: main.cpp
- Revision:
- 0:98c7c6fe50cc
- Child:
- 1:491e67888f28
diff -r 000000000000 -r 98c7c6fe50cc main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Mar 16 15:06:44 2017 +0000 @@ -0,0 +1,330 @@ +#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; +}