CORE / Mbed 2 deprecated 201703_NUPERION_VENESUS

ヌペリオン星人の偵察型UAV「ヴェネサス」．その飛行アルゴリズムを解析し，C言語にて再現したもの．

Dependencies:   MPU6050_2 MS5607 SDFileSystem mbed

Fork of 00NUPERION_ver12 by naoki nishii

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?

UserRevisionLine numberNew 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 }