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