あああ
Dependencies: MPU6050_2 MS5607 SDFileSystem mbed
main.cpp@0:98c7c6fe50cc, 2017-03-16 (annotated)
- Committer:
- naoki_westwell
- Date:
- Thu Mar 16 15:06:44 2017 +0000
- Revision:
- 0:98c7c6fe50cc
- Child:
- 1:491e67888f28
?????uav?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
naoki_westwell | 0:98c7c6fe50cc | 1 | #include "mbed.h" |
naoki_westwell | 0:98c7c6fe50cc | 2 | #include "MS5607I2C.h" |
naoki_westwell | 0:98c7c6fe50cc | 3 | #include "SDFileSystem.h" |
naoki_westwell | 0:98c7c6fe50cc | 4 | #include "MPU6050.h" |
naoki_westwell | 0:98c7c6fe50cc | 5 | #include <stdio.h> |
naoki_westwell | 0:98c7c6fe50cc | 6 | |
naoki_westwell | 0:98c7c6fe50cc | 7 | /***************************/ |
naoki_westwell | 0:98c7c6fe50cc | 8 | /* 打上前に値を確認!!!!! */ |
naoki_westwell | 0:98c7c6fe50cc | 9 | /***************************/ |
naoki_westwell | 0:98c7c6fe50cc | 10 | #define L_HOKUI 35.546257 // 10進法 KBIC 3本の樹 |
naoki_westwell | 0:98c7c6fe50cc | 11 | #define L_TOUKEI 139.671875 |
naoki_westwell | 0:98c7c6fe50cc | 12 | #define LNCH_ACC 2 // 発射判定閾値[G] |
naoki_westwell | 0:98c7c6fe50cc | 13 | #define TRANS_TIME 50 // 打上から制御開始までの時間[sec/10] |
naoki_westwell | 0:98c7c6fe50cc | 14 | /***************************/ |
naoki_westwell | 0:98c7c6fe50cc | 15 | |
naoki_westwell | 0:98c7c6fe50cc | 16 | #define DEG_TO_RAD(x) ( x * 0.01745329 ) |
naoki_westwell | 0:98c7c6fe50cc | 17 | #define RAD_TO_DEG(x) ( x * 57.29578 ) |
naoki_westwell | 0:98c7c6fe50cc | 18 | |
naoki_westwell | 0:98c7c6fe50cc | 19 | #define HIGH 1 |
naoki_westwell | 0:98c7c6fe50cc | 20 | #define LOW 0 |
naoki_westwell | 0:98c7c6fe50cc | 21 | |
naoki_westwell | 0:98c7c6fe50cc | 22 | #define STDBY_MODE 0 // 待機 |
naoki_westwell | 0:98c7c6fe50cc | 23 | #define TRANS_MODE 1 // 輸送 |
naoki_westwell | 0:98c7c6fe50cc | 24 | #define CTRL_MODE 2 // 制御 |
naoki_westwell | 0:98c7c6fe50cc | 25 | |
naoki_westwell | 0:98c7c6fe50cc | 26 | #define GPS_RATE 10 // GPSの取得レート |
naoki_westwell | 0:98c7c6fe50cc | 27 | #define SAVE_RATE 50 // SDへの保存の間隔[sec/10] |
naoki_westwell | 0:98c7c6fe50cc | 28 | |
naoki_westwell | 0:98c7c6fe50cc | 29 | #define ACC_SSF 2048 //Sensitivity Scale Factor |
naoki_westwell | 0:98c7c6fe50cc | 30 | #define GYRO_SSF 16.4 |
naoki_westwell | 0:98c7c6fe50cc | 31 | #define PRESS_INIT 10 // 基準気圧の平均を取る数 |
naoki_westwell | 0:98c7c6fe50cc | 32 | |
naoki_westwell | 0:98c7c6fe50cc | 33 | #define KPL 2.0 // ラダーゲイン |
naoki_westwell | 0:98c7c6fe50cc | 34 | #define KPE 2.0 // エレベータゲイン |
naoki_westwell | 0:98c7c6fe50cc | 35 | |
naoki_westwell | 0:98c7c6fe50cc | 36 | #define GOAL_R 50 // 目標地点にこれだけ近づいたら制御方法変える |
naoki_westwell | 0:98c7c6fe50cc | 37 | #define DELTA_E_FULLUP 2000 |
naoki_westwell | 0:98c7c6fe50cc | 38 | #define DELTA_E_NTRL 1500 |
naoki_westwell | 0:98c7c6fe50cc | 39 | #define DELTA_E_FULLDOWN 1000 |
naoki_westwell | 0:98c7c6fe50cc | 40 | #define DELTA_L_FULLR 2000 |
naoki_westwell | 0:98c7c6fe50cc | 41 | #define DELTA_L_NTRL 1500 |
naoki_westwell | 0:98c7c6fe50cc | 42 | #define DELTA_L_FULLL 1000 |
naoki_westwell | 0:98c7c6fe50cc | 43 | |
naoki_westwell | 0:98c7c6fe50cc | 44 | #define PI 3.14159265358979 // π |
naoki_westwell | 0:98c7c6fe50cc | 45 | #define A_RADIUS 6378137.000 // 地球長半径 [m] |
naoki_westwell | 0:98c7c6fe50cc | 46 | #define B_RADIUS 6356752.314245 // 地球短半径 (WGS84)[m] |
naoki_westwell | 0:98c7c6fe50cc | 47 | #define ECC2 0.006694379990 // 離心率の二乗 |
naoki_westwell | 0:98c7c6fe50cc | 48 | |
naoki_westwell | 0:98c7c6fe50cc | 49 | /* ペリフェラル宣言 */ |
naoki_westwell | 0:98c7c6fe50cc | 50 | Serial pc(USBTX,USBRX); |
naoki_westwell | 0:98c7c6fe50cc | 51 | Serial gps(p9, p10); |
naoki_westwell | 0:98c7c6fe50cc | 52 | Serial twe(p13, p14); |
naoki_westwell | 0:98c7c6fe50cc | 53 | PwmOut eServo(p21); // ラダー用サーボ |
naoki_westwell | 0:98c7c6fe50cc | 54 | PwmOut lServo(p22); // エレベータ用サーボ |
naoki_westwell | 0:98c7c6fe50cc | 55 | DigitalOut camSw(p20); // カメラスイッチ |
naoki_westwell | 0:98c7c6fe50cc | 56 | SDFileSystem sd(p5, p6, p7, p8, "sd"); |
naoki_westwell | 0:98c7c6fe50cc | 57 | MPU6050 mpu; |
naoki_westwell | 0:98c7c6fe50cc | 58 | MS5607I2C ms5607(p28, p27, true); |
naoki_westwell | 0:98c7c6fe50cc | 59 | BusOut leds(LED1, LED2, LED3, LED4); |
naoki_westwell | 0:98c7c6fe50cc | 60 | |
naoki_westwell | 0:98c7c6fe50cc | 61 | /* グローバル変数宣言 */ |
naoki_westwell | 0:98c7c6fe50cc | 62 | int mode = STDBY_MODE; |
naoki_westwell | 0:98c7c6fe50cc | 63 | int cnt = 0; |
naoki_westwell | 0:98c7c6fe50cc | 64 | |
naoki_westwell | 0:98c7c6fe50cc | 65 | int gps_i; // 文字数,モード(センテンス終わりか?判定) |
naoki_westwell | 0:98c7c6fe50cc | 66 | char gps_data[256]; |
naoki_westwell | 0:98c7c6fe50cc | 67 | float beta, alpha; |
naoki_westwell | 0:98c7c6fe50cc | 68 | float set_point_l, set_point_e; |
naoki_westwell | 0:98c7c6fe50cc | 69 | float delta_l=1500, delta_e=1500; |
naoki_westwell | 0:98c7c6fe50cc | 70 | |
naoki_westwell | 0:98c7c6fe50cc | 71 | FILE *fp; |
naoki_westwell | 0:98c7c6fe50cc | 72 | char all_data[256]; |
naoki_westwell | 0:98c7c6fe50cc | 73 | |
naoki_westwell | 0:98c7c6fe50cc | 74 | /* プロトタイプ宣言 */ |
naoki_westwell | 0:98c7c6fe50cc | 75 | bool Init(); |
naoki_westwell | 0:98c7c6fe50cc | 76 | void getData(); |
naoki_westwell | 0:98c7c6fe50cc | 77 | float getAlt(float press, float press0, float temp); |
naoki_westwell | 0:98c7c6fe50cc | 78 | //void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[]); |
naoki_westwell | 0:98c7c6fe50cc | 79 | void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz); |
naoki_westwell | 0:98c7c6fe50cc | 80 | void filter_vz(double dt, double C, double &vz_est, double vz); |
naoki_westwell | 0:98c7c6fe50cc | 81 | |
naoki_westwell | 0:98c7c6fe50cc | 82 | bool Init() |
naoki_westwell | 0:98c7c6fe50cc | 83 | { |
naoki_westwell | 0:98c7c6fe50cc | 84 | gps.baud(9600); // ボーレート |
naoki_westwell | 0:98c7c6fe50cc | 85 | pc.baud(115200); |
naoki_westwell | 0:98c7c6fe50cc | 86 | twe.baud(115200); |
naoki_westwell | 0:98c7c6fe50cc | 87 | eServo.period_us(20000); // servo requires a 20ms period |
naoki_westwell | 0:98c7c6fe50cc | 88 | lServo.period_us(20000); |
naoki_westwell | 0:98c7c6fe50cc | 89 | lServo.pulsewidth_us(0); |
naoki_westwell | 0:98c7c6fe50cc | 90 | eServo.pulsewidth_us(0); |
naoki_westwell | 0:98c7c6fe50cc | 91 | |
naoki_westwell | 0:98c7c6fe50cc | 92 | 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だけにする |
naoki_westwell | 0:98c7c6fe50cc | 93 | gps.printf("$PMTK300,100,0,0,0,0*2C\r\n"); // position fixを100msecに(レート10spsにする) |
naoki_westwell | 0:98c7c6fe50cc | 94 | |
naoki_westwell | 0:98c7c6fe50cc | 95 | mpu.initialize(); |
naoki_westwell | 0:98c7c6fe50cc | 96 | ms5607.printCoefficients(); |
naoki_westwell | 0:98c7c6fe50cc | 97 | |
naoki_westwell | 0:98c7c6fe50cc | 98 | pc.puts("Init finish!\n"); |
naoki_westwell | 0:98c7c6fe50cc | 99 | return true; |
naoki_westwell | 0:98c7c6fe50cc | 100 | } |
naoki_westwell | 0:98c7c6fe50cc | 101 | |
naoki_westwell | 0:98c7c6fe50cc | 102 | /******************/ |
naoki_westwell | 0:98c7c6fe50cc | 103 | // GPSの取得周期(10SPS)に沿って,他のデータ取得等の処理を行う. |
naoki_westwell | 0:98c7c6fe50cc | 104 | /******************/ |
naoki_westwell | 0:98c7c6fe50cc | 105 | |
naoki_westwell | 0:98c7c6fe50cc | 106 | void getData() |
naoki_westwell | 0:98c7c6fe50cc | 107 | { |
naoki_westwell | 0:98c7c6fe50cc | 108 | char c = gps.getc(); |
naoki_westwell | 0:98c7c6fe50cc | 109 | if( c=='$' || gps_i == 256) { |
naoki_westwell | 0:98c7c6fe50cc | 110 | gps_i = 0; |
naoki_westwell | 0:98c7c6fe50cc | 111 | } |
naoki_westwell | 0:98c7c6fe50cc | 112 | if((gps_data[gps_i]=c) != '\r') { |
naoki_westwell | 0:98c7c6fe50cc | 113 | gps_i++; |
naoki_westwell | 0:98c7c6fe50cc | 114 | } else { |
naoki_westwell | 0:98c7c6fe50cc | 115 | |
naoki_westwell | 0:98c7c6fe50cc | 116 | gps_data[gps_i]='\0'; |
naoki_westwell | 0:98c7c6fe50cc | 117 | |
naoki_westwell | 0:98c7c6fe50cc | 118 | float w_time, hokui, tokei; // 時刻,北緯,東経 |
naoki_westwell | 0:98c7c6fe50cc | 119 | float vb, beta2; // 速度,真方位 |
naoki_westwell | 0:98c7c6fe50cc | 120 | char ns, ew, status; // NS,EW,その他 |
naoki_westwell | 0:98c7c6fe50cc | 121 | if( sscanf(gps_data, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,",&w_time,&status,&hokui,&ns,&tokei,&ew,&vb,&beta2) >= 1) { |
naoki_westwell | 0:98c7c6fe50cc | 122 | //Longitude |
naoki_westwell | 0:98c7c6fe50cc | 123 | double d_tokei= int(tokei/100); |
naoki_westwell | 0:98c7c6fe50cc | 124 | double m_tokei= (tokei-d_tokei*100)/60; |
naoki_westwell | 0:98c7c6fe50cc | 125 | double g_tokei= d_tokei+m_tokei; // 10進法に換算 |
naoki_westwell | 0:98c7c6fe50cc | 126 | //Latitude |
naoki_westwell | 0:98c7c6fe50cc | 127 | double d_hokui= int(hokui/100); |
naoki_westwell | 0:98c7c6fe50cc | 128 | double m_hokui= (hokui-d_hokui*100)/60; |
naoki_westwell | 0:98c7c6fe50cc | 129 | double g_hokui= d_hokui+m_hokui; |
naoki_westwell | 0:98c7c6fe50cc | 130 | /* ヒュベニの公式 */ |
naoki_westwell | 0:98c7c6fe50cc | 131 | double dy = (L_HOKUI-g_hokui)/180*PI; //ラジアン |
naoki_westwell | 0:98c7c6fe50cc | 132 | double dx = (L_TOUKEI-g_tokei)/180*PI; |
naoki_westwell | 0:98c7c6fe50cc | 133 | double yAve = (g_hokui+L_HOKUI)/2/180*PI; |
naoki_westwell | 0:98c7c6fe50cc | 134 | double w = sqrt(1-ECC2*sin(yAve)*sin(yAve)); |
naoki_westwell | 0:98c7c6fe50cc | 135 | double m = A_RADIUS*(1-ECC2)/(w*w*w); |
naoki_westwell | 0:98c7c6fe50cc | 136 | double n = A_RADIUS/w; |
naoki_westwell | 0:98c7c6fe50cc | 137 | double distance = sqrt((dy*m)*(dy*m)+(dx*n*cos(yAve))*(dx*n*cos(yAve))); |
naoki_westwell | 0:98c7c6fe50cc | 138 | double xDist = dx*n*cos(yAve); |
naoki_westwell | 0:98c7c6fe50cc | 139 | double yDist = dy*m; |
naoki_westwell | 0:98c7c6fe50cc | 140 | |
naoki_westwell | 0:98c7c6fe50cc | 141 | /* 高度 */ |
naoki_westwell | 0:98c7c6fe50cc | 142 | static float alt, alt0; //前の高度 |
naoki_westwell | 0:98c7c6fe50cc | 143 | static float press_arr[PRESS_INIT]; // 基準高度はこれの平均をとる |
naoki_westwell | 0:98c7c6fe50cc | 144 | static float press0 = 0; |
naoki_westwell | 0:98c7c6fe50cc | 145 | float temp = ms5607.getTemperature(); // 気圧 |
naoki_westwell | 0:98c7c6fe50cc | 146 | float press = ms5607.getPressure(); // 温度 |
naoki_westwell | 0:98c7c6fe50cc | 147 | |
naoki_westwell | 0:98c7c6fe50cc | 148 | /* 基準電圧の取得 */ |
naoki_westwell | 0:98c7c6fe50cc | 149 | if(mode == STDBY_MODE) { |
naoki_westwell | 0:98c7c6fe50cc | 150 | for(int i=0; i<PRESS_INIT-1; i++) press_arr[i] = press_arr[i+1]; // 気圧データをシフト |
naoki_westwell | 0:98c7c6fe50cc | 151 | press_arr[PRESS_INIT-1] = press; |
naoki_westwell | 0:98c7c6fe50cc | 152 | } |
naoki_westwell | 0:98c7c6fe50cc | 153 | if(mode == TRANS_MODE) { //発射直後,基準電圧を計算 |
naoki_westwell | 0:98c7c6fe50cc | 154 | if(cnt==0 && press0==0) { |
naoki_westwell | 0:98c7c6fe50cc | 155 | for(int i=0; i<PRESS_INIT; i++) press0 += press_arr[i]; |
naoki_westwell | 0:98c7c6fe50cc | 156 | press0 /= PRESS_INIT; |
naoki_westwell | 0:98c7c6fe50cc | 157 | } |
naoki_westwell | 0:98c7c6fe50cc | 158 | } |
naoki_westwell | 0:98c7c6fe50cc | 159 | |
naoki_westwell | 0:98c7c6fe50cc | 160 | double vz_calc = (alt - alt0) / 0.1; |
naoki_westwell | 0:98c7c6fe50cc | 161 | alt0 = alt; |
naoki_westwell | 0:98c7c6fe50cc | 162 | alt = getAlt(press, press0, temp); //高度の算出 |
naoki_westwell | 0:98c7c6fe50cc | 163 | double alt_est = 0.0, vz_est = 0.0; |
naoki_westwell | 0:98c7c6fe50cc | 164 | float dt = 1/GPS_RATE; |
naoki_westwell | 0:98c7c6fe50cc | 165 | filter_alt(dt, 0.1, alt_est, alt, vz_est); |
naoki_westwell | 0:98c7c6fe50cc | 166 | filter_vz(dt, 0.01, vz_est, vz_calc); |
naoki_westwell | 0:98c7c6fe50cc | 167 | |
naoki_westwell | 0:98c7c6fe50cc | 168 | /* 加速度 */ |
naoki_westwell | 0:98c7c6fe50cc | 169 | int16_t ax, ay, az; // mpu生データ |
naoki_westwell | 0:98c7c6fe50cc | 170 | int16_t gx, gy, gz; |
naoki_westwell | 0:98c7c6fe50cc | 171 | mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
naoki_westwell | 0:98c7c6fe50cc | 172 | float fax = float(ax)/ACC_SSF; // mpu物理量に換算 |
naoki_westwell | 0:98c7c6fe50cc | 173 | float fay = float(ay)/ACC_SSF; |
naoki_westwell | 0:98c7c6fe50cc | 174 | float faz = float(az)/ACC_SSF; |
naoki_westwell | 0:98c7c6fe50cc | 175 | //float fgx = float(gx)/GYRO_SSF; |
naoki_westwell | 0:98c7c6fe50cc | 176 | //float fgy =float(gy)/GYRO_SSF; |
naoki_westwell | 0:98c7c6fe50cc | 177 | //float fgz = float(gz)/GYRO_SSF; |
naoki_westwell | 0:98c7c6fe50cc | 178 | float resAcc = sqrt(fax*fax + fay*fay + faz*faz); //合成加速度 |
naoki_westwell | 0:98c7c6fe50cc | 179 | |
naoki_westwell | 0:98c7c6fe50cc | 180 | /* アングルオフ */ |
naoki_westwell | 0:98c7c6fe50cc | 181 | if(beta2 > 180) beta2 -= 360; |
naoki_westwell | 0:98c7c6fe50cc | 182 | double beta3 = RAD_TO_DEG(atan2(yDist, xDist)); |
naoki_westwell | 0:98c7c6fe50cc | 183 | beta = beta2 - beta3; |
naoki_westwell | 0:98c7c6fe50cc | 184 | if(beta > 180) beta -= 360; |
naoki_westwell | 0:98c7c6fe50cc | 185 | if(beta <-180) beta += 360; |
naoki_westwell | 0:98c7c6fe50cc | 186 | |
naoki_westwell | 0:98c7c6fe50cc | 187 | /* 降下角 */ |
naoki_westwell | 0:98c7c6fe50cc | 188 | alpha = -atan(vz_est/vb); |
naoki_westwell | 0:98c7c6fe50cc | 189 | |
naoki_westwell | 0:98c7c6fe50cc | 190 | /* サーボ操作 P制御 */ |
naoki_westwell | 0:98c7c6fe50cc | 191 | if(distance <= GOAL_R){ |
naoki_westwell | 0:98c7c6fe50cc | 192 | // ラダー操舵 |
naoki_westwell | 0:98c7c6fe50cc | 193 | set_point_l = DELTA_L_NTRL + 500*(beta * KPL / 180); |
naoki_westwell | 0:98c7c6fe50cc | 194 | if(set_point_l > DELTA_L_FULLR) set_point_l = DELTA_L_FULLR; |
naoki_westwell | 0:98c7c6fe50cc | 195 | if(set_point_l < DELTA_L_FULLL) set_point_l = DELTA_L_FULLL; |
naoki_westwell | 0:98c7c6fe50cc | 196 | if((set_point_l-delta_l)>=0){ |
naoki_westwell | 0:98c7c6fe50cc | 197 | delta_l += 100; |
naoki_westwell | 0:98c7c6fe50cc | 198 | if((set_point_l-delta_l)<0) delta_l = set_point_l; |
naoki_westwell | 0:98c7c6fe50cc | 199 | }else{ |
naoki_westwell | 0:98c7c6fe50cc | 200 | delta_l -= 100; |
naoki_westwell | 0:98c7c6fe50cc | 201 | if((set_point_l-delta_l)>0) delta_l = set_point_l+1; |
naoki_westwell | 0:98c7c6fe50cc | 202 | } |
naoki_westwell | 0:98c7c6fe50cc | 203 | |
naoki_westwell | 0:98c7c6fe50cc | 204 | // エレベータ制御 |
naoki_westwell | 0:98c7c6fe50cc | 205 | set_point_e = DELTA_E_NTRL + 500*(alpha * KPE / 180); |
naoki_westwell | 0:98c7c6fe50cc | 206 | if(set_point_e > DELTA_E_FULLUP) set_point_e = DELTA_E_FULLUP; |
naoki_westwell | 0:98c7c6fe50cc | 207 | if(set_point_e < DELTA_E_FULLDOWN) set_point_e = DELTA_E_FULLDOWN; |
naoki_westwell | 0:98c7c6fe50cc | 208 | if((set_point_e-delta_e)>=0){ |
naoki_westwell | 0:98c7c6fe50cc | 209 | delta_e += 100; |
naoki_westwell | 0:98c7c6fe50cc | 210 | if((set_point_e-delta_e)<0) delta_e = set_point_e; |
naoki_westwell | 0:98c7c6fe50cc | 211 | }else{ |
naoki_westwell | 0:98c7c6fe50cc | 212 | delta_e -= 100; |
naoki_westwell | 0:98c7c6fe50cc | 213 | if((set_point_e-delta_e)>0) delta_e = set_point_e+1; |
naoki_westwell | 0:98c7c6fe50cc | 214 | } |
naoki_westwell | 0:98c7c6fe50cc | 215 | }else{ |
naoki_westwell | 0:98c7c6fe50cc | 216 | if(cnt%10) delta_e = DELTA_E_NTRL; |
naoki_westwell | 0:98c7c6fe50cc | 217 | else delta_e = DELTA_E_FULLUP; |
naoki_westwell | 0:98c7c6fe50cc | 218 | delta_l = DELTA_L_FULLR; // 要検討 |
naoki_westwell | 0:98c7c6fe50cc | 219 | } |
naoki_westwell | 0:98c7c6fe50cc | 220 | lServo.pulsewidth_us(delta_l); |
naoki_westwell | 0:98c7c6fe50cc | 221 | eServo.pulsewidth_us(delta_e); |
naoki_westwell | 0:98c7c6fe50cc | 222 | |
naoki_westwell | 0:98c7c6fe50cc | 223 | /* 取得データの整理と出力 */ |
naoki_westwell | 0:98c7c6fe50cc | 224 | sprintf(all_data, "%6lf,%.2f,%.2f,%.2f,%d,%d\r\n", |
naoki_westwell | 0:98c7c6fe50cc | 225 | distance, vb, beta, alt, cnt, mode); |
naoki_westwell | 0:98c7c6fe50cc | 226 | pc.printf("%s", all_data); |
naoki_westwell | 0:98c7c6fe50cc | 227 | if(!(cnt%10)) twe.printf("%.6f,%.6f,%.2f,%d\r\n", g_tokei, g_hokui, alt, cnt); |
naoki_westwell | 0:98c7c6fe50cc | 228 | //pc.printf("%s\r\n",gps_data); |
naoki_westwell | 0:98c7c6fe50cc | 229 | //pc.printf("%f,%f\r\n", delta_l, set_point_l); |
naoki_westwell | 0:98c7c6fe50cc | 230 | |
naoki_westwell | 0:98c7c6fe50cc | 231 | /* カメラスイッチ操作 */ |
naoki_westwell | 0:98c7c6fe50cc | 232 | if(mode==TRANS_MODE){ |
naoki_westwell | 0:98c7c6fe50cc | 233 | if(cnt<=20) camSw = HIGH; // 2秒間長押しで電源オンの後,一回短押しで撮影開始 |
naoki_westwell | 0:98c7c6fe50cc | 234 | if(cnt>20 && cnt<=25) camSw = LOW; |
naoki_westwell | 0:98c7c6fe50cc | 235 | if(cnt>25 && cnt<=30) camSw = HIGH; |
naoki_westwell | 0:98c7c6fe50cc | 236 | if(cnt>30) camSw = LOW; |
naoki_westwell | 0:98c7c6fe50cc | 237 | } |
naoki_westwell | 0:98c7c6fe50cc | 238 | |
naoki_westwell | 0:98c7c6fe50cc | 239 | /* SDカードに保存 */ |
naoki_westwell | 0:98c7c6fe50cc | 240 | if((cnt%SAVE_RATE)==0) fp = fopen("/sd/myfile.txt", "a"); |
naoki_westwell | 0:98c7c6fe50cc | 241 | fprintf(fp, "%s", all_data); |
naoki_westwell | 0:98c7c6fe50cc | 242 | if((cnt%SAVE_RATE)==SAVE_RATE-1) fclose(fp); |
naoki_westwell | 0:98c7c6fe50cc | 243 | sprintf(gps_data, ""); |
naoki_westwell | 0:98c7c6fe50cc | 244 | |
naoki_westwell | 0:98c7c6fe50cc | 245 | /* カウント */ |
naoki_westwell | 0:98c7c6fe50cc | 246 | cnt++; |
naoki_westwell | 0:98c7c6fe50cc | 247 | if(cnt>=600) cnt = 0; |
naoki_westwell | 0:98c7c6fe50cc | 248 | |
naoki_westwell | 0:98c7c6fe50cc | 249 | /* モード遷移判定 */ |
naoki_westwell | 0:98c7c6fe50cc | 250 | if(mode == STDBY_MODE) { // 待機⇒制御 |
naoki_westwell | 0:98c7c6fe50cc | 251 | if(resAcc > LNCH_ACC) { |
naoki_westwell | 0:98c7c6fe50cc | 252 | mode = TRANS_MODE; |
naoki_westwell | 0:98c7c6fe50cc | 253 | cnt = 0; |
naoki_westwell | 0:98c7c6fe50cc | 254 | } |
naoki_westwell | 0:98c7c6fe50cc | 255 | leds = 1 << cnt%4; |
naoki_westwell | 0:98c7c6fe50cc | 256 | }else |
naoki_westwell | 0:98c7c6fe50cc | 257 | if(mode == TRANS_MODE) { |
naoki_westwell | 0:98c7c6fe50cc | 258 | if(cnt > TRANS_TIME) { |
naoki_westwell | 0:98c7c6fe50cc | 259 | mode = CTRL_MODE; |
naoki_westwell | 0:98c7c6fe50cc | 260 | cnt = 0; |
naoki_westwell | 0:98c7c6fe50cc | 261 | } |
naoki_westwell | 0:98c7c6fe50cc | 262 | leds = 3 << cnt%4; |
naoki_westwell | 0:98c7c6fe50cc | 263 | }else |
naoki_westwell | 0:98c7c6fe50cc | 264 | if(mode == CTRL_MODE) { |
naoki_westwell | 0:98c7c6fe50cc | 265 | leds = 15*(cnt%2); |
naoki_westwell | 0:98c7c6fe50cc | 266 | } |
naoki_westwell | 0:98c7c6fe50cc | 267 | } |
naoki_westwell | 0:98c7c6fe50cc | 268 | } |
naoki_westwell | 0:98c7c6fe50cc | 269 | } |
naoki_westwell | 0:98c7c6fe50cc | 270 | |
naoki_westwell | 0:98c7c6fe50cc | 271 | int main() |
naoki_westwell | 0:98c7c6fe50cc | 272 | { |
naoki_westwell | 0:98c7c6fe50cc | 273 | Init(); |
naoki_westwell | 0:98c7c6fe50cc | 274 | pc.printf("*** Start up! ***\r\n"); |
naoki_westwell | 0:98c7c6fe50cc | 275 | gps.attach(getData,Serial::RxIrq); |
naoki_westwell | 0:98c7c6fe50cc | 276 | while(1) {} |
naoki_westwell | 0:98c7c6fe50cc | 277 | } |
naoki_westwell | 0:98c7c6fe50cc | 278 | // 高度計算 |
naoki_westwell | 0:98c7c6fe50cc | 279 | float getAlt(float press, float press0, float temp) |
naoki_westwell | 0:98c7c6fe50cc | 280 | { |
naoki_westwell | 0:98c7c6fe50cc | 281 | return (pow((press0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; |
naoki_westwell | 0:98c7c6fe50cc | 282 | } |
naoki_westwell | 0:98c7c6fe50cc | 283 | |
naoki_westwell | 0:98c7c6fe50cc | 284 | // 位置(速度)フィルタ |
naoki_westwell | 0:98c7c6fe50cc | 285 | void filter_x(double dt, double C, double x_est[], double x_gps[], double vg_gps[]) |
naoki_westwell | 0:98c7c6fe50cc | 286 | { |
naoki_westwell | 0:98c7c6fe50cc | 287 | static double x0[] = { x_gps[0], x_gps[1], x_gps[2] }; |
naoki_westwell | 0:98c7c6fe50cc | 288 | static int called = 0; |
naoki_westwell | 0:98c7c6fe50cc | 289 | if (!called) { |
naoki_westwell | 0:98c7c6fe50cc | 290 | for (int i = 0; i < 2; i++) |
naoki_westwell | 0:98c7c6fe50cc | 291 | x_est[i] = x0[i]; |
naoki_westwell | 0:98c7c6fe50cc | 292 | called++; |
naoki_westwell | 0:98c7c6fe50cc | 293 | return; |
naoki_westwell | 0:98c7c6fe50cc | 294 | } |
naoki_westwell | 0:98c7c6fe50cc | 295 | double xt[2]; |
naoki_westwell | 0:98c7c6fe50cc | 296 | for (int i = 0; i < 2; i++) { |
naoki_westwell | 0:98c7c6fe50cc | 297 | xt[i] = x0[i] + vg_gps[i] * dt; |
naoki_westwell | 0:98c7c6fe50cc | 298 | x_est[i] = C*x_gps[i] + (1 - C)*xt[i]; |
naoki_westwell | 0:98c7c6fe50cc | 299 | x0[i] = x_est[i]; |
naoki_westwell | 0:98c7c6fe50cc | 300 | } |
naoki_westwell | 0:98c7c6fe50cc | 301 | } |
naoki_westwell | 0:98c7c6fe50cc | 302 | // 高度フィルタ |
naoki_westwell | 0:98c7c6fe50cc | 303 | void filter_alt(double dt, double C, double &alt_est, double alt_s, double vz) |
naoki_westwell | 0:98c7c6fe50cc | 304 | { |
naoki_westwell | 0:98c7c6fe50cc | 305 | static double alt0 = alt_s; |
naoki_westwell | 0:98c7c6fe50cc | 306 | static int called = 0; |
naoki_westwell | 0:98c7c6fe50cc | 307 | if (!called) { |
naoki_westwell | 0:98c7c6fe50cc | 308 | alt_est = alt0; |
naoki_westwell | 0:98c7c6fe50cc | 309 | called++; |
naoki_westwell | 0:98c7c6fe50cc | 310 | return; |
naoki_westwell | 0:98c7c6fe50cc | 311 | } |
naoki_westwell | 0:98c7c6fe50cc | 312 | double altt = alt0 + vz * dt; |
naoki_westwell | 0:98c7c6fe50cc | 313 | { |
naoki_westwell | 0:98c7c6fe50cc | 314 | alt_est = C*alt_s + (1 - C)*altt; |
naoki_westwell | 0:98c7c6fe50cc | 315 | alt0 = alt_est; |
naoki_westwell | 0:98c7c6fe50cc | 316 | } |
naoki_westwell | 0:98c7c6fe50cc | 317 | } |
naoki_westwell | 0:98c7c6fe50cc | 318 | // 垂直速度フィルタ |
naoki_westwell | 0:98c7c6fe50cc | 319 | void filter_vz(double dt, double C, double &vz_est, double vz) |
naoki_westwell | 0:98c7c6fe50cc | 320 | { |
naoki_westwell | 0:98c7c6fe50cc | 321 | static double vz0 = 0.0; |
naoki_westwell | 0:98c7c6fe50cc | 322 | static int called = 0; |
naoki_westwell | 0:98c7c6fe50cc | 323 | if (!called) { |
naoki_westwell | 0:98c7c6fe50cc | 324 | vz_est = vz0; |
naoki_westwell | 0:98c7c6fe50cc | 325 | called++; |
naoki_westwell | 0:98c7c6fe50cc | 326 | return; |
naoki_westwell | 0:98c7c6fe50cc | 327 | } |
naoki_westwell | 0:98c7c6fe50cc | 328 | vz_est = C*vz + (1 - C)*vz0; |
naoki_westwell | 0:98c7c6fe50cc | 329 | vz0 = vz_est; |
naoki_westwell | 0:98c7c6fe50cc | 330 | } |