main.cpp

00001 #include "mbed.h"
00002 #include "mpu9250_i2c.h"
00003 #include "MadgwickFilter.hpp"
00004 #include "LPS25H_lib.h"
00005 #include "GPS_interrupt.h"
00006 #include "string.h"
00007 
00008 /**************************************
00009 コンストラクタ
00010 **************************************/
00011 Serial pc(USBTX, USBRX, 115200);
00012 Serial GPS(p9, p10, 115200);
00013 
00014 LocalFileSystem local("local");
00015 
00016 DigitalOut led_lps(LED1);
00017 DigitalOut led_mpu(LED2);
00018 DigitalOut led_gps(LED3);
00019 DigitalOut led_log(LED4);
00020 AnalogIn vinread(p15);
00021 PwmOut buzzer(p21);
00022 I2C m_i2c(p28, p27);
00023 
00024 mpu9250 nine(m_i2c, AD0_HIGH);
00025 MadgwickFilter attitude(1.0);
00026 LPS25H_lib LPS25H(LPS25H_lib::AD0_HIGH, m_i2c);
00027 GPS_interrupt mGPS(&GPS, 115200);
00028 
00029 Ticker tick_print;
00030 Ticker tick_rec;
00031 Ticker tick_pipi;
00032 Ticker tick_gps;
00033 Ticker tick_lps;
00034 InterruptIn myButton(p18);
00035 Timer timer_button;
00036 
00037 /**************************************
00038 変数の宣言
00039 **************************************/
00040 float gyro[3], gyro_rad[3], acc[3], mag[3], euler[3];
00041 Quaternion quart(1.0, 0.0, 0.0, 0.0);
00042 float pres, temp, alt, pres_0, temp_0, speed, pres_old;
00043 float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg;
00044 int gps_sat;
00045 float vin;
00046 
00047 bool pipi_now, save, button, top_flight, sec_flight;
00048 bool flag_test_gps = true;  //GPSの設定をするか
00049 
00050 FILE *fp;
00051 int time_button;
00052 
00053 /**************************************
00054 関数のプロトタイプ宣言
00055 **************************************/
00056 void setup();
00057 void printData();
00058 void resetPT();
00059 void startRec();
00060 void stopRec();
00061 void recData();
00062 void pipi();
00063 void lvup();
00064 void tone(float freq);
00065 
00066 void readGPS();
00067 void readLPS();
00068 void readMPU();
00069 float deg2rad(float deg);
00070 float rad2deg(float rad);
00071 
00072 void buttonPush();
00073 void buttonRelease();
00074 
00075 /**************************************
00076 メイン関数
00077 **************************************/
00078 int main() {
00079     setup();
00080     lvup();
00081     
00082     myButton.mode(PullUp);
00083     myButton.fall(&buttonPush);
00084     myButton.rise(&buttonRelease);
00085     
00086     tick_print.attach(&printData, 1.0f);
00087     tick_gps.attach(&readGPS, 0.1f);
00088     tick_lps.attach(&readLPS, 0.05f);
00089     
00090     while(1) {
00091         readMPU();
00092     }
00093 }
00094 
00095 /**************************************
00096 データの表示
00097 **************************************/
00098 void printData(){
00099     for(int i = 0; i < 20; i ++){
00100         pc.printf("*");
00101     }
00102     pc.printf("\r\n");
00103     pc.printf("TIME    -> %2d:%2d:%02.2f\r\n", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]);
00104     pc.printf("GPS     -> %3.7f(N), %3.7f(E), %.2f[m]\r\n", gps_lat, gps_lon, gps_alt);
00105     pc.printf("SPEED   -> %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
00106     pc.printf("SAT     -> %d\r\n", gps_sat);
00107     pc.printf("LPS25H  -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt);
00108     pc.printf("SPEED   -> %.4f[m/s]\r\n", speed);
00109     pc.printf("BATTERY -> %.2f[V]\r\n", vin);
00110     pc.printf("GYRO    -> %.4f, %.4f, %.4f\r\n", gyro[0], gyro[1], gyro[2]);
00111     pc.printf("ACC     -> %.4f, %.4f, %.4f\r\n", acc[0], acc[1], acc[2]);
00112     pc.printf("MAG     -> %.2f, %.2f, %.2f\r\n", mag[0], mag[1], mag[2]);
00113     pc.printf("QUART   -> %f, %f, %f, %f\r\n", quart.w, quart.x, quart.y, quart.z);
00114     pc.printf("EULER   -> %f, %f, %f\r\n", euler[0], euler[1], euler[2]);
00115     pc.printf("TOP     -> %d, %d\r\n", top_flight, sec_flight);
00116     pc.printf("SAVE    -> %d\r\n\n\n", save);
00117 }
00118 
00119 /**************************************
00120 高度のリセット
00121 **************************************/
00122 void resetPT(){
00123     pres_0 = pres;
00124     temp_0 = temp;
00125     lvup();
00126 }
00127 
00128 /**************************************
00129 記録開始
00130 **************************************/
00131 void startRec(){
00132     fp = fopen("/local/rocketlog.txt", "a");
00133     pc.printf("START SAVING\r\n");
00134     led_log = 1;
00135     wait(1.0f);
00136 }
00137 
00138 /**************************************
00139 記録終了
00140 **************************************/
00141 void stopRec(){
00142     fprintf(fp, "\r\n\n");
00143     fclose(fp);
00144     pc.printf("STOP SAVING\r\n");
00145     led_log = 0;
00146     wait(1.0f);
00147 }
00148 
00149 /**************************************
00150 記録書き込み
00151 **************************************/
00152 void recData(){
00153     fprintf(fp, "%d/%d/%d,", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]); //日付 
00154     fprintf(fp, "%2d:%2d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間
00155     fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標
00156     fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度
00157     fprintf(fp, "%d,", gps_sat); //GPS衛星数
00158     fprintf(fp, "%.4f,%.2f,%.2f,", pres, temp, alt); //気圧センサ
00159     fprintf(fp, "%.4f,", speed); //Z軸速度
00160     fprintf(fp, "%.2f,", vin); //電源電圧
00161     fprintf(fp, "%.4f,%.4f,%.4f,", gyro[0], gyro[1], gyro[2]); //ジャイロ
00162     fprintf(fp, "%.4f,%.4f,%.4f,", acc[0], acc[1], acc[2]); //加速度
00163     fprintf(fp, "%.2f,%.2f,%.2f,", mag[0], mag[1], mag[2]); //地磁気
00164     fprintf(fp, "%f,%f,%f,%f,", quart.w, quart.x, quart.y, quart.z); //クオータニオン
00165     fprintf(fp, "%f,%f,%f,", euler[0], euler[1], euler[2]); //姿勢角
00166     fprintf(fp, "%d,%d\r\n", top_flight, sec_flight); //頂点検知
00167 }
00168 
00169 /**************************************
00170 ブザー
00171 **************************************/
00172 void pipi(){
00173     if(pipi_now){
00174         buzzer = 0.0f;
00175         pipi_now = !pipi_now;
00176     }
00177     else{
00178         float T = 1.0f / (4.0*440.000f);
00179         buzzer.period(T);
00180         buzzer = 0.5f;
00181         pipi_now = !pipi_now;
00182     }
00183 }
00184 
00185 /**************************************
00186 レベルアップ
00187 **************************************/
00188 void lvup(){
00189     for(int i = 1; i <= 4; i ++){
00190         tone(4.0*349.228f);
00191         wait(0.15f);
00192     }
00193     tone(0.0f);
00194     wait(0.15f);
00195     tone(4.0*311.127f);
00196     wait(0.15f);
00197     tone(0.0f);
00198     wait(0.15f);
00199     tone(4.0*391.995f);
00200     wait(0.15f);
00201     tone(0.0f);
00202     wait(0.15f);
00203     tone(4.0*349.228f);
00204     wait(1.0f);
00205     tone(0.0f);
00206     wait(1.0f);
00207 }
00208 
00209 /**************************************
00210 音を鳴らす
00211 **************************************/
00212 void tone(float freq){
00213     float T = 0;
00214     
00215     if(freq == 0.0f){
00216         buzzer = 0.0f;
00217     }
00218     
00219     T = 1.0f / freq;
00220     buzzer.period(T);
00221     buzzer = 0.5f;
00222 }
00223 
00224 /**************************************
00225 GPS読み取り
00226 **************************************/
00227 void readGPS(){
00228     gps_lat = mGPS.Latitude();
00229     gps_lon = mGPS.Longitude();
00230     gps_alt = mGPS.Height();
00231     mGPS.getUTC(gps_utc);
00232     gps_utc[3] += 9;
00233     if(gps_utc[3] >= 24){
00234         gps_utc[3] -= 24;
00235     }
00236     gps_knot = mGPS.Knot();
00237     gps_deg = mGPS.Degree();
00238     gps_sat = mGPS.Number();
00239     
00240     vin = vinread.read() * 8.4;
00241 }
00242 
00243 /**************************************
00244 気圧センサー読み取り・速度計算
00245 **************************************/
00246 void readLPS(){
00247     pres_old = pres;
00248     pres = LPS25H.getPres();
00249     temp = LPS25H.getTemp();
00250     alt = LPS25H.getAlt(pres_0, temp_0);
00251     speed = (pres - pres_old) / 0.05;
00252     
00253     int top_c = 0;
00254     if(alt > 70 && speed < 3.0f && !top_flight){
00255         top_c ++;
00256         if(top_c > 10){
00257             top_c = 0;
00258             top_flight = true;
00259         }
00260     }
00261     if(alt < 30 && speed < 0.0f && top_flight && !sec_flight){
00262         top_c ++;
00263         if(top_c > 10){
00264             top_c = 0;
00265             sec_flight = true;
00266         }
00267     }
00268 }
00269 
00270 /**************************************
00271 9軸センサー読み取り・姿勢計算
00272 **************************************/
00273 void readMPU(){
00274     nine.getGyro(gyro);
00275     nine.getAcc(acc);
00276     nine.getMag(mag);
00277     
00278     for(int i = 0; i < 3; i ++){
00279         gyro_rad[i] = deg2rad(gyro[i]);
00280     }
00281     attitude.MadgwickAHRSupdate(gyro_rad, acc, mag);
00282     attitude.getAttitude(&quart);
00283     attitude.getEulerAngle(euler);
00284     for(int i = 0; i < 3; i ++){
00285         euler[i] = rad2deg(euler[i]);
00286     }
00287 }
00288 
00289 /**************************************
00290 度数法 -> 弧度法
00291 **************************************/
00292 float deg2rad(float deg){
00293     return deg * 0.017453292519943; //* 3.1415926535 / 180;
00294 }
00295 
00296 /**************************************
00297 弧度法 -> 度数法
00298 **************************************/
00299 float rad2deg(float rad){
00300     return rad * 57.29577951308232; //* 180 / 3.145926535;
00301 }
00302 
00303 /**************************************
00304 ボタンが押されたとき
00305 **************************************/
00306 void buttonPush(){
00307     if(!button){
00308         button = true;
00309         timer_button.reset();
00310         timer_button.start();
00311     }
00312 }
00313 
00314 /**************************************
00315 ボタンが離されたとき
00316 **************************************/
00317 void buttonRelease(){
00318     if(button){
00319         button = false;
00320         time_button = timer_button.read();
00321         timer_button.stop();
00322         if(time_button >= 2){
00323             if(!save){
00324                 save = true;
00325                 startRec();
00326                 tick_rec.detach();
00327                 tick_rec.attach(&recData, 0.05f);
00328                 tick_pipi.attach(&pipi, 0.5f);
00329             }
00330             else{
00331                 save = false;
00332                 stopRec();
00333                 tick_rec.detach();
00334                 tick_pipi.detach();
00335                 tone(0.0f);
00336             }
00337         }
00338         else{
00339             resetPT();
00340         }
00341     }
00342 }
00343 
00344 /**************************************
00345 セットアップ（最初に1回実行）
00346 **************************************/
00347 void setup(){
00348     wait(0.5f);
00349     char setup_string[1024];
00350     
00351     pc.printf("\r\n\nSetting Start\r\n");//
00352     strcat(setup_string, "\r\n\nSetting Start\r\n");
00353     
00354     LPS25H.begin(25);
00355     LPS25H.setFIFO(16);
00356     if(LPS25H.whoAmI() == 0){
00357         pc.printf("LPS25H OK!!\r\n");//
00358         strcat(setup_string, "LPS25H OK!!\r\n");
00359         led_lps = 1;
00360     }
00361     else{
00362         pc.printf("LPS25H NG...\r\n");//
00363         strcat(setup_string, "LPS25H NG...\r\n");
00364         led_lps = 0;
00365     }
00366     
00367     nine.setAcc(_16G);
00368     nine.setGyro(_2000DPS);
00369     nine.setOffset(-2.17859, -0.11247, 0.924360,
00370                    0.005295, 0.019147, 0.056065,
00371                    49.425, -5.55, 75.15);
00372     int check_nine = 0;
00373     if(nine.senserTest()){
00374         pc.printf("MPU9250 OK!!\r\n");//
00375         strcat(setup_string, "MPU9250 OK!!\r\n");
00376         check_nine ++;
00377     }
00378     else{
00379         pc.printf("MPU9250 NG...\r\n");//
00380         strcat(setup_string, "MPU9250 NG...\r\n");
00381     }
00382     if(nine.mag_senserTest()){
00383         pc.printf("MPU9250 MAG OK!!\r\n");//
00384         strcat(setup_string, "MPU9250 MAG OK!!\r\n");
00385         check_nine ++;
00386     }
00387     else{
00388         pc.printf("MPU9250 MAG NG...\r\n");//
00389         strcat(setup_string, "MPU9250 MAG NG...\r\n");
00390     }
00391     if(check_nine == 2){
00392         led_mpu = 1;
00393     }
00394     else{
00395         led_mpu = 0;
00396     }
00397     
00398     if(flag_test_gps){
00399         pc.printf("GPS setting");//
00400         strcat(setup_string, "GPS setting");
00401         led_gps = 0;
00402         
00403         while(!mGPS.gps_readable){
00404             pc.printf(".");//
00405             strcat(setup_string, ".");
00406             wait(1.0f);
00407         }
00408         
00409         pc.printf(" Finished!!\r\n");//
00410         strcat(setup_string, " Finished!!\r\n");
00411         led_gps = 1;
00412     }
00413     
00414     pc.printf("All setting finished!! -> Start!!\r\n");//
00415     strcat(setup_string, "All setting finished!! -> Start!!\r\n");
00416     
00417     led_log = 1;
00418     fp = fopen("/local/rocketlog.txt", "a");
00419     fprintf(fp, setup_string);
00420     fclose(fp);
00421     setup_string[0] = NULL;
00422     led_log = 0;
00423 }