File content as of revision 5:03d31f0a4943:

#include "mbed.h"
#include "LPS331_I2C.h"
#include "LSM9DS1.h"
#include "SDFileSystem.h"
#include "IM920.h"
#include "millis.h"
#include "GPS.h"
#include "LPS22HB.h"
#include "stdio.h"

//ピン
DigitalIn flightpin(p12);
LPS331_I2C lps331(p9, p10, LPS331_I2C_SA0_HIGH);
LSM9DS1 lsm(p9,p10);
SDFileSystem sd(p5, p6, p7, p8, "sd");
IM920 im920(p28, p27, p29, p30);
GPS gps(p13, p14);
LPS22HB lps33hw(p9,p10,LPS22HB_G_CHIP_ADDR);
Timer waitTime;
char sendData[256];
PwmOut servo_1(p22), servo_2(p25);
bool SDisAvailable=false;

//関数
void init(FILE **file);//諸々の初期化
void checkingSensors();
void readValues(int sequence);//センサの読み取り
float calcAltitude(float pres, float temp);//高度計算

int main() {
    int sequence=0;//機体の状態の推移
    int pretime=0,pretime_2=0;
    float altitude,maxAltitude=0.0;//m単位
    float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持
    char receive[9]={};//受信した文字列
    bool flightpinAttached=false;//フライトピンが付いているかどうか
    millisStart();

    FILE *fp;
    //サーボclose
    servo_1.pulsewidth(1.05/1000.0);//ms/1000
    servo_2.pulsewidth(1.94/1000.0);
    
    init(&fp);//初期化
    flightpinAttached=flightpin;
    
    while(flightpinAttached == false){
        im920.poll();
        memset(receive,'\0',sizeof(receive));
        im920.recv(receive,8);
        flightpinAttached=flightpin;
        if(millis()-pretime>5000){
            im920.send("Waiting_1",10);
            pretime=millis();
        }
        if(strncmp(receive,"nowait",sizeof(receive))==0){
            flightpinAttached=false;
            break;
        }
        if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
            im920.send("Retry initializing",19);
            init(&fp);
            wait(1);
        }
        if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
            im920.send("Mbed reset",11);
            NVIC_SystemReset();
        }
    }
    while(flightpinAttached == true){//組み立て中はつけたまま
        im920.poll();
        memset(receive,'\0',sizeof(receive));
        im920.recv(receive,8);
        flightpinAttached=flightpin;
        if(millis()-pretime>5000){
            im920.send("Waiting_2",10);
            pretime=millis();
        }
        if(strncmp(receive,"nowait",sizeof(receive))==0){
            flightpinAttached=false;
            break;
        }
        if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
            im920.send("Retry initializing",19);
            init(&fp);
            wait(1);
        }
        if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
            im920.send("Mbed reset",11);
            NVIC_SystemReset();
        }
    }
    while(flightpinAttached == false){//ランチャー取り付け中は取り外す
        im920.poll();
        memset(receive,'\0',sizeof(receive));
        im920.recv(receive,8);
        flightpinAttached=flightpin;
        if(millis()-pretime>5000){
            im920.send("Waiting_3",10);
            pretime=millis();
        }
        if(strncmp(receive,"nowait",sizeof(receive))==0){
            flightpinAttached=false;
            break;
        }
        if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
            im920.send("Retry initializing",19);
            init(&fp);
            wait(1);
        }
        if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
            im920.send("Mbed reset",11);
            NVIC_SystemReset();
        }
    }
    im920.send("escape Waiting",14);
    
    while(strncmp(receive,"start",sizeof(receive))!=0){//無線での開始指令待ち txda startと受信したら計測開始
        im920.poll();
        memset(receive,'\0',sizeof(receive));
        im920.recv(receive,8);
        //加速度・フライトピン読み取り
        lsm.readAccel();
        flightpinAttached=flightpin;
        if(strncmp(receive,"check",sizeof(receive))==0){//checkと受信したらセンサーの状態確認 主にGPS
            im920.send("Check",6);
            checkingSensors();
        }
        if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
            im920.send("Retry initializing",19);
            init(&fp);
            wait(1);
        }
        if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
            im920.send("Mbed reset",11);
            NVIC_SystemReset();
        }
        if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=4.0 && flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始
            im920.send("Forced start",13);
            launchTime=millis();
            sequence=1;
            break;
        }
        //地上局への送信
        if(millis() - pretime > 5000){//5秒おきに経過時間送信
            lps33hw.get();
            sprintf(sendData," %06.3fs, %06.2fPa",(float)millis()/1000,lps33hw.pressure());
            im920.send(sendData,20);
            pretime = millis();
        }
    }
    im920.send("START",6);
        
    while(strncmp(receive,"end",sizeof(receive))!=0&&sequence!=3) {//endと受信するまたは着地したら終了
        //受信内容の初期化
        im920.poll();
        memset(receive,'\0',sizeof(receive));
        im920.recv(receive,8);
        
        if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
            im920.send("Mbed reset",11);
            NVIC_SystemReset();
        }
        
        //センサ・フライトピン読み取り 高度計測
        readValues(sequence);
        flightpinAttached=flightpin;
        altitude=calcAltitude(lps33hw.pressure(),lps33hw.temperature());
        
        if(maxAltitude<altitude){//最高高度更新
            maxAltitude=altitude;
        }
        
        switch(sequence){
            case 0://発射待機中
                //離床検知
                if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=4.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている
                    im920.send("LAUNCH ACC>2.0G",16);
                    launchTime=millis();
                    sequence=1;
                }
                if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=3.9*3.9 &&flightpinAttached==true){//加速度4.0G以上かつフライトピンがぬけてない
                    im920.send("LAUNCH ACC>3.9G",16);
                    launchTime=millis();
                    sequence=1;
                }
                break;
                
            case 1://発射～減速機構展開
                //減速機構展開
                if(millis()-launchTime>=10*1000){
                    if(millis()-launchTime>=15*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下
                        im920.send("Start speed reducing",21);
                        reducerExpantionedTime=millis();
                        //サーボopen
                        servo_1.pulsewidth(1.90/1000);//ms/1000
                        servo_2.pulsewidth(1.32/1000);
                        sequence=2;
                    }
                }
                break;
                
            case 2://減速機構展開～着地
                if(millis()-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過
                    im920.send("LANDING",8);
                    sequence=3;
                }
                break;
        }
        
        //SD書き込み time,temp,pres,temp_hw,pres_hw,accXYZ,gyroXYZ,magnXYZ,longitude,latitude
        if(SDisAvailable){            
            if(sequence==1){//飛行中
                fprintf(fp,"%.3f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
                (float)millis()/1000,lps331.getTemperature(),lps331.getPressure(),lps33hw.temperature(),lps33hw.pressure(),lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz);
            }else{//飛行中以外はGPS情報も保存
                fprintf(fp,"%.3f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n",
                (float)millis()/1000,lps331.getTemperature(),lps331.getPressure(),lps33hw.temperature(),lps33hw.pressure(),lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz,gps.longitude,gps.latitude);
            }
        }
        if(millis() - pretime_2 > 2000){//2秒おきにGPS送信
            sprintf(sendData," %1d: %06.2fm, %07.3fs",sequence,altitude,(float)millis()/1000);
            im920.send(sendData,22);
            pretime_2=millis();
        }
        
        //地上局へのGPS情報送信
        if(millis() - pretime > 5000){//5秒おきにGPS送信
            if(sequence != 1){
                waitTime.start();
                waitTime.reset();
                while(gps.result==false){//GPSが利用可能になるまでまつ
                    gps.getgps();
                    if(waitTime.read() > 1){
                        waitTime.stop();
                        break;
                    }
                }
                sprintf(sendData," %08.5fE, %08.5fN",gps.longitude,gps.latitude);
                im920.send(sendData,24);
            }
            pretime=millis();
        }
    }
    if(SDisAvailable){
        fclose(fp);
        im920.send("File closed",12);
    }else{
        im920.send("SD error",9);
    }
    sprintf(sendData," MaxAltitude: %06.2fm",maxAltitude);
    im920.send(sendData,23);
    while(1){
        //受信内容の初期化
        im920.poll();
        memset(receive,'\0',sizeof(receive));
        im920.recv(receive,8);
        
        if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
            im920.send("Mbed reset",11);
            NVIC_SystemReset();
        }
        
        if(millis() - pretime > 5000){//5秒おきにGPS送信
            if(sequence != 1){
                waitTime.start();
                waitTime.reset();
                while(gps.result==false){//GPSが利用可能になるまでまつ
                    gps.getgps();
                    if(waitTime.read() > 1){
                        waitTime.stop();
                        break;
                    }
                }
                sprintf(sendData," %08.5fE, %08.5fN",gps.longitude,gps.latitude);
                im920.send(sendData,24);
            }
            pretime=millis();
        }
    }
}

void init(FILE **file){
     //im920
    im920.init();
    wait(1);
    
    //MMTXS03
    if(lps331.isAvailable()) {
        im920.send("MM-TXS03...OK",14);
    } else {
        im920.send("MM-TXS03...NG",14);
    }
    
    //MM9DS1
    lsm.begin();
    if(lsm.whoAmI()){
        im920.send("MM-9DS1...OK",13);
    }else {
        im920.send("MM-9DS1...NG",13);
    }
    
    //LPS33HW
    if(!lps33hw.whoAmI()){
        im920.send("LPS33HW...OK",13);
    }else {
        im920.send("LPS33HW...NG",13);
    }
    wait(1);
    
    //SD
    *file = fopen("/sd/data.csv", "w");
    if(*file == NULL) {
        im920.send("SD...NG",8);
        SDisAvailable = false;
    }else{
        im920.send("SD...OK",8);
        fprintf(*file,"time,temperature,pressure,temperature_hw,pressure_hw,accX,accY,accZ,gyroX,gyroY,gyroZ,magnX,magnY,magnZ,longitude,latitude\r\n");
        SDisAvailable = true;
    }
    
    //GPS優先度
    NVIC_SetPriority(UART3_IRQn,0);
    gps.getgps();
    if(gps.result){
        im920.send("GPS...OK",9);
        sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
        im920.send(sendData,11);
    }else{
        im920.send("GPS...NG",9);
    }
    
    //フライトピン
    //flightpin.output();
    
    //各種設定
    /*lps331.setResolution(LPS331_I2C_PRESSURE_AVG_512, LPS331_I2C_TEMP_AVG_128);
    lps331.setDataRate(LPS331_I2C_DATARATE_25HZ_T);
    lps331.setActive(true);
    lsm.setAccelODR(lsm.A_ODR_952);
    lsm.setGyroODR(lsm.G_ODR_952_BW_100);
    lsm.setMagODR(lsm.M_ODR_80);*/
}

void checkingSensors(){
    //MMTXS03
    if(lps331.isAvailable()) {
        im920.send("MM-TXS03...OK",14);
    } else {
        im920.send("MM-TXS03...NG",14);
    }
    
    //MM9DS1
    if(lsm.whoAmI()){
        im920.send("MM-9DS1...OK",13);
    }else {
        im920.send("MM-9DS1...NG",13);
    }
    
    //LPS33HW
    if(!lps33hw.whoAmI()){
        im920.send("LPS33HW...OK",13);
    }else {
        im920.send("LPS33HW...NG",13);
    }
    
    //GPS
    gps.getgps();
    if(gps.result){
        im920.send("GPS...OK",9);
        sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
        im920.send(sendData,11);
    }else{
        im920.send("GPS...NG",9);
    }
}

void readValues(int sequence){
    lsm.readAccel();
    lsm.readGyro();
    lsm.readMag();
    lps331.getPressure();
    lps331.getTemperature();
    lps33hw.get();
    if(sequence!=1){
        gps.getgps();
    }
}

float calcAltitude(float pres, float temp){
    return (pow(1013.25/pres,1/5.257)-1)*(temp+273.15)/0.0065;
}