File content as of revision 0:6a155e0d095d:

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