Diff: main.cpp

Revision:

0:6a155e0d095d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 24 12:25:58 2018 +0000
@@ -0,0 +1,423 @@
+#include "mbed.h"
+#include "mpu9250_i2c.h"
+#include "MadgwickFilter.hpp"
+#include "LPS25H_lib.h"
+#include "GPS_interrupt.h"
+#include "string.h"
+
+/**************************************
+コンストラクタ
+**************************************/
+Serial pc(USBTX, USBRX, 115200);
+Serial GPS(p9, p10, 115200);
+
+LocalFileSystem local("local");
+
+DigitalOut led_lps(LED1);
+DigitalOut led_mpu(LED2);
+DigitalOut led_gps(LED3);
+DigitalOut led_log(LED4);
+AnalogIn vinread(p15);
+PwmOut buzzer(p21);
+I2C m_i2c(p28, p27);
+
+mpu9250 nine(m_i2c, AD0_HIGH);
+MadgwickFilter attitude(1.0);
+LPS25H_lib LPS25H(LPS25H_lib::AD0_HIGH, m_i2c);
+GPS_interrupt mGPS(&GPS, 115200);
+
+Ticker tick_print;
+Ticker tick_rec;
+Ticker tick_pipi;
+Ticker tick_gps;
+Ticker tick_lps;
+InterruptIn myButton(p18);
+Timer timer_button;
+
+/**************************************
+変数の宣言
+**************************************/
+float gyro[3], gyro_rad[3], acc[3], mag[3], euler[3];
+Quaternion quart(1.0, 0.0, 0.0, 0.0);
+float pres, temp, alt, pres_0, temp_0, speed, pres_old;
+float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg;
+int gps_sat;
+float vin;
+
+bool pipi_now, save, button, top_flight, sec_flight;
+bool flag_test_gps = true;  //GPSの設定をするか
+
+FILE *fp;
+int time_button;
+
+/**************************************
+関数のプロトタイプ宣言
+**************************************/
+void setup();
+void printData();
+void resetPT();
+void startRec();
+void stopRec();
+void recData();
+void pipi();
+void lvup();
+void tone(float freq);
+
+void readGPS();
+void readLPS();
+void readMPU();
+float deg2rad(float deg);
+float rad2deg(float rad);
+
+void buttonPush();
+void buttonRelease();
+
+/**************************************
+メイン関数
+**************************************/
+int main() {
+    setup();
+    lvup();
+    
+    myButton.mode(PullUp);
+    myButton.fall(&buttonPush);
+    myButton.rise(&buttonRelease);
+    
+    tick_print.attach(&printData, 1.0f);
+    tick_gps.attach(&readGPS, 0.1f);
+    tick_lps.attach(&readLPS, 0.05f);
+    
+    while(1) {
+        readMPU();
+    }
+}
+
+/**************************************
+データの表示
+**************************************/
+void printData(){
+    for(int i = 0; i < 20; i ++){
+        pc.printf("*");
+    }
+    pc.printf("\r\n");
+    pc.printf("TIME    -> %2d:%2d:%02.2f\r\n", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]);
+    pc.printf("GPS     -> %3.7f(N), %3.7f(E), %.2f[m]\r\n", gps_lat, gps_lon, gps_alt);
+    pc.printf("SPEED   -> %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
+    pc.printf("SAT     -> %d\r\n", gps_sat);
+    pc.printf("LPS25H  -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt);
+    pc.printf("SPEED   -> %.4f[m/s]\r\n", speed);
+    pc.printf("BATTERY -> %.2f[V]\r\n", vin);
+    pc.printf("GYRO    -> %.4f, %.4f, %.4f\r\n", gyro[0], gyro[1], gyro[2]);
+    pc.printf("ACC     -> %.4f, %.4f, %.4f\r\n", acc[0], acc[1], acc[2]);
+    pc.printf("MAG     -> %.2f, %.2f, %.2f\r\n", mag[0], mag[1], mag[2]);
+    pc.printf("QUART   -> %f, %f, %f, %f\r\n", quart.w, quart.x, quart.y, quart.z);
+    pc.printf("EULER   -> %f, %f, %f\r\n", euler[0], euler[1], euler[2]);
+    pc.printf("TOP     -> %d, %d\r\n", top_flight, sec_flight);
+    pc.printf("SAVE    -> %d\r\n\n\n", save);
+}
+
+/**************************************
+高度のリセット
+**************************************/
+void resetPT(){
+    pres_0 = pres;
+    temp_0 = temp;
+    lvup();
+}
+
+/**************************************
+記録開始
+**************************************/
+void startRec(){
+    fp = fopen("/local/rocketlog.txt", "a");
+    pc.printf("START SAVING\r\n");
+    led_log = 1;
+    wait(1.0f);
+}
+
+/**************************************
+記録終了
+**************************************/
+void stopRec(){
+    fprintf(fp, "\r\n\n");
+    fclose(fp);
+    pc.printf("STOP SAVING\r\n");
+    led_log = 0;
+    wait(1.0f);
+}
+
+/**************************************
+記録書き込み
+**************************************/
+void recData(){
+    fprintf(fp, "%d/%d/%d,", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]); //日付 
+    fprintf(fp, "%2d:%2d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間
+    fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標
+    fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度
+    fprintf(fp, "%d,", gps_sat); //GPS衛星数
+    fprintf(fp, "%.4f,%.2f,%.2f,", pres, temp, alt); //気圧センサ
+    fprintf(fp, "%.4f,", speed); //Z軸速度
+    fprintf(fp, "%.2f,", vin); //電源電圧
+    fprintf(fp, "%.4f,%.4f,%.4f,", gyro[0], gyro[1], gyro[2]); //ジャイロ
+    fprintf(fp, "%.4f,%.4f,%.4f,", acc[0], acc[1], acc[2]); //加速度
+    fprintf(fp, "%.2f,%.2f,%.2f,", mag[0], mag[1], mag[2]); //地磁気
+    fprintf(fp, "%f,%f,%f,%f,", quart.w, quart.x, quart.y, quart.z); //クオータニオン
+    fprintf(fp, "%f,%f,%f,", euler[0], euler[1], euler[2]); //姿勢角
+    fprintf(fp, "%d,%d\r\n", top_flight, sec_flight); //頂点検知
+}
+
+/**************************************
+ブザー
+**************************************/
+void pipi(){
+    if(pipi_now){
+        buzzer = 0.0f;
+        pipi_now = !pipi_now;
+    }
+    else{
+        float T = 1.0f / (4.0*440.000f);
+        buzzer.period(T);
+        buzzer = 0.5f;
+        pipi_now = !pipi_now;
+    }
+}
+
+/**************************************
+レベルアップ
+**************************************/
+void lvup(){
+    for(int i = 1; i <= 4; i ++){
+        tone(4.0*349.228f);
+        wait(0.15f);
+    }
+    tone(0.0f);
+    wait(0.15f);
+    tone(4.0*311.127f);
+    wait(0.15f);
+    tone(0.0f);
+    wait(0.15f);
+    tone(4.0*391.995f);
+    wait(0.15f);
+    tone(0.0f);
+    wait(0.15f);
+    tone(4.0*349.228f);
+    wait(1.0f);
+    tone(0.0f);
+    wait(1.0f);
+}
+
+/**************************************
+音を鳴らす
+**************************************/
+void tone(float freq){
+    float T = 0;
+    
+    if(freq == 0.0f){
+        buzzer = 0.0f;
+    }
+    
+    T = 1.0f / freq;
+    buzzer.period(T);
+    buzzer = 0.5f;
+}
+
+/**************************************
+GPS読み取り
+**************************************/
+void readGPS(){
+    gps_lat = mGPS.Latitude();
+    gps_lon = mGPS.Longitude();
+    gps_alt = mGPS.Height();
+    mGPS.getUTC(gps_utc);
+    gps_utc[3] += 9;
+    if(gps_utc[3] >= 24){
+        gps_utc[3] -= 24;
+    }
+    gps_knot = mGPS.Knot();
+    gps_deg = mGPS.Degree();
+    gps_sat = mGPS.Number();
+    
+    vin = vinread.read() * 8.4;
+}
+
+/**************************************
+気圧センサー読み取り・速度計算
+**************************************/
+void readLPS(){
+    pres_old = pres;
+    pres = LPS25H.getPres();
+    temp = LPS25H.getTemp();
+    alt = LPS25H.getAlt(pres_0, temp_0);
+    speed = (pres - pres_old) / 0.05;
+    
+    int top_c = 0;
+    if(alt > 70 && speed < 3.0f && !top_flight){
+        top_c ++;
+        if(top_c > 10){
+            top_c = 0;
+            top_flight = true;
+        }
+    }
+    if(alt < 30 && speed < 0.0f && top_flight && !sec_flight){
+        top_c ++;
+        if(top_c > 10){
+            top_c = 0;
+            sec_flight = true;
+        }
+    }
+}
+
+/**************************************
+9軸センサー読み取り・姿勢計算
+**************************************/
+void readMPU(){
+    nine.getGyro(gyro);
+    nine.getAcc(acc);
+    nine.getMag(mag);
+    
+    for(int i = 0; i < 3; i ++){
+        gyro_rad[i] = deg2rad(gyro[i]);
+    }
+    attitude.MadgwickAHRSupdate(gyro_rad, acc, mag);
+    attitude.getAttitude(&quart);
+    attitude.getEulerAngle(euler);
+    for(int i = 0; i < 3; i ++){
+        euler[i] = rad2deg(euler[i]);
+    }
+}
+
+/**************************************
+度数法 -> 弧度法
+**************************************/
+float deg2rad(float deg){
+    return deg * 0.017453292519943; //* 3.1415926535 / 180;
+}
+
+/**************************************
+弧度法 -> 度数法
+**************************************/
+float rad2deg(float rad){
+    return rad * 57.29577951308232; //* 180 / 3.145926535;
+}
+
+/**************************************
+ボタンが押されたとき
+**************************************/
+void buttonPush(){
+    if(!button){
+        button = true;
+        timer_button.reset();
+        timer_button.start();
+    }
+}
+
+/**************************************
+ボタンが離されたとき
+**************************************/
+void buttonRelease(){
+    if(button){
+        button = false;
+        time_button = timer_button.read();
+        timer_button.stop();
+        if(time_button >= 2){
+            if(!save){
+                save = true;
+                startRec();
+                tick_rec.detach();
+                tick_rec.attach(&recData, 0.05f);
+                tick_pipi.attach(&pipi, 0.5f);
+            }
+            else{
+                save = false;
+                stopRec();
+                tick_rec.detach();
+                tick_pipi.detach();
+                tone(0.0f);
+            }
+        }
+        else{
+            resetPT();
+        }
+    }
+}
+
+/**************************************
+セットアップ（最初に1回実行）
+**************************************/
+void setup(){
+    wait(0.5f);
+    char setup_string[1024];
+    
+    pc.printf("\r\n\nSetting Start\r\n");//
+    strcat(setup_string, "\r\n\nSetting Start\r\n");
+    
+    LPS25H.begin(25);
+    LPS25H.setFIFO(16);
+    if(LPS25H.whoAmI() == 0){
+        pc.printf("LPS25H OK!!\r\n");//
+        strcat(setup_string, "LPS25H OK!!\r\n");
+        led_lps = 1;
+    }
+    else{
+        pc.printf("LPS25H NG...\r\n");//
+        strcat(setup_string, "LPS25H NG...\r\n");
+        led_lps = 0;
+    }
+    
+    nine.setAcc(_16G);
+    nine.setGyro(_2000DPS);
+    nine.setOffset(-2.17859, -0.11247, 0.924360,
+                   0.005295, 0.019147, 0.056065,
+                   49.425, -5.55, 75.15);
+    int check_nine = 0;
+    if(nine.senserTest()){
+        pc.printf("MPU9250 OK!!\r\n");//
+        strcat(setup_string, "MPU9250 OK!!\r\n");
+        check_nine ++;
+    }
+    else{
+        pc.printf("MPU9250 NG...\r\n");//
+        strcat(setup_string, "MPU9250 NG...\r\n");
+    }
+    if(nine.mag_senserTest()){
+        pc.printf("MPU9250 MAG OK!!\r\n");//
+        strcat(setup_string, "MPU9250 MAG OK!!\r\n");
+        check_nine ++;
+    }
+    else{
+        pc.printf("MPU9250 MAG NG...\r\n");//
+        strcat(setup_string, "MPU9250 MAG NG...\r\n");
+    }
+    if(check_nine == 2){
+        led_mpu = 1;
+    }
+    else{
+        led_mpu = 0;
+    }
+    
+    if(flag_test_gps){
+        pc.printf("GPS setting");//
+        strcat(setup_string, "GPS setting");
+        led_gps = 0;
+        
+        while(!mGPS.gps_readable){
+            pc.printf(".");//
+            strcat(setup_string, ".");
+            wait(1.0f);
+        }
+        
+        pc.printf(" Finished!!\r\n");//
+        strcat(setup_string, " Finished!!\r\n");
+        led_gps = 1;
+    }
+    
+    pc.printf("All setting finished!! -> Start!!\r\n");//
+    strcat(setup_string, "All setting finished!! -> Start!!\r\n");
+    
+    led_log = 1;
+    fp = fopen("/local/rocketlog.txt", "a");
+    fprintf(fp, setup_string);
+    fclose(fp);
+    setup_string[0] = NULL;
+    led_log = 0;
+}