naoki nishii / Mbed 2 deprecated 00NUPERION_ver12

あああ

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?

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