File content as of revision 0:cebf1f73ffd5:

#include "mbed.h"
#include <string.h>
#include "LPS331_I2C.h"
#include "LSM9DS1.h"
#include "SDFileSystem.h"
#include "IM920.h"
#include "GPS.h"
#include "LPS22HB.h"

//ピン
DigitalInOut flightpin(p12);
Serial pc(USBTX, USBRX);
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);
//I2C i2c(p9,p10);
//LPS22HB lps33hw(i2c, LPS22HB_G_CHIP_ADDR);
PwmOut servo_1(p22), servo_2(p25);

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

int main() {
    int sequence=0;//機体の状態の推移
    int timer=0;//ms単位
    const int timeInterval=50;//ms単位
    float altitude,maxAltitude=0.0;//m単位
    float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持
    char receive[9]={};//受信した文字列
    bool flightpinAttached=false;//フライトピンが付いているかどうか
    FILE *fp;
    
    init(&fp);//初期化
    
    //サーボclose
    //servo_1.pulsewidth(0.76/1000);//ms/1000
    //servo_2.pulsewidth(1.94/1000);
    
    while(strncmp(receive,"start",sizeof(receive))!=0){//無線での開始指令待ち txda tartと受信したら計測開始
        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);
        }
        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);
            break;
        }
    }
    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(lps331.pressure,lps331.temperature);
        
        pc.printf("%.2f\r\n",(float)timer/1000);
        /*pc.printf("%f,%f\r\n", lps331.pressure, lps331.temperature);
        pc.printf("accX=%.3f / accY=%.3f / accZ=%.3f\r\n",lsm.ax,lsm.ay,lsm.az);
        pc.printf("gyroX=%.3f / gyroY=%.3f / gyroZ=%.3f\r\n",lsm.gx,lsm.gy,lsm.gz);
        pc.printf("magnX=%.3f / magnY=%.3f / magnZ=%.3f\r\n",lsm.mx,lsm.my,lsm.mz);
        pc.printf("\r\n");*/
        
        //地上局への送信
        if(timer%1000==0){//1秒おきに経過時間送信
            im920.send(valueToChar((float)timer/1000),7);
        }
        
        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=timer;
                    sequence=1;
                }
                break;
                
            case 1://発射～減速機構展開
                //減速機構展開
                if(timer-launchTime>=18*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下
                    im920.send("Start speed reducing",21);
                    reducerExpantionedTime=timer;
                    //サーボopen
                    //servo_1.pulsewidth(1.28/1000);//ms/1000
                    //servo_2.pulsewidth(1.32/1000);
                    sequence=2;
                }
                break;
                
            case 2://減速機構展開～着地
                if(timer-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(sequence==1){//飛行中
            fprintf(fp,"%.2f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
            (float)timer/1000,lps331.temperature,lps331.pressure,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,"%.2f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n",
            (float)timer/1000,lps331.temperature,lps331.pressure,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);
        }
        
        timer+=timeInterval;
        wait((float)timeInterval/1000);
    }
    wait(0.5);
    fclose(fp);
    wait(0.5);
    im920.send("File closed",12);
    im920.send("MaxAltitude=",13);
    im920.send(valueToChar(maxAltitude),7);
    
    //地上局へのGPS情報送信
    im920.send("Waiting for GPS response",25);
    while(gps.result==false){//GPSが利用可能になるまでまつ
        gps.getgps();
    }
    im920.send(valueToChar(gps.longitude),7);
    im920.send(valueToChar(gps.latitude),7);
}

void init(FILE **file){
     //im920
    im920.init();
    wait(1);
    
    //MMTXS03
    if(lps331.isAvailable()) {
        pc.printf("LPS331 is available\r\n");
        im920.send("MM-TXS03...OK",14);
    } else {
        pc.printf("LPS331 is unavailable\r\n");
        im920.send("MM-TXS03...NG",14);
    }
    
    //MM9DS1
    lsm.begin();
    if(lsm.whoAmI()){
        pc.printf("LSM9DS1 is available\r\n");
        im920.send("MM-9DS1...OK",13);
    }else {
        pc.printf("LSM9DS1 is unavailable\r\n");
        im920.send("MM-9DS1...NG",13);
    }
    
    //LPS33HW
    if(lps33hw.whoAmI()){
        pc.printf("LPS33HW is available\r\n");
        im920.send("LPS33HW...OK",13);
    }else {
        pc.printf("LPS33HW is unavailable\r\n");
        im920.send("LPS33HW...NG",13);
    }
    
    //SD
    if(*file!=NULL){
        fclose(*file);
    }
    *file = fopen("/sd/data.csv", "w");
    if(*file == NULL) {
        pc.printf("Could not open file for write\r\n");
        im920.send("SD...NG",8);
    }else{
        pc.printf("file opend\r\n");
        im920.send("SD...OK",8);
        fprintf(*file,"time,temperature,pressure,accX,accY,accZ,gyroX,gyroY,gyroZ,magnX,magnY,magnZ,longitude,latitude\r\n");
    }
    
    //GPS優先度
    NVIC_SetPriority(UART3_IRQn,0);
    gps.getgps();
    if(gps.result){
        pc.printf("GPS is available\r\n");
        im920.send("GPS...OK",9);
        im920.send(valueToChar(gps.longitude),7);
        im920.send(valueToChar(gps.latitude),7);
    }else{
        pc.printf("GPS is unavailable\r\n");
        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()) {
        pc.printf("LPS331 is available\r\n");
        im920.send("MM-TXS03...OK",14);
    } else {
        pc.printf("LPS331 is unavailable\r\n");
        im920.send("MM-TXS03...NG",14);
    }
    
    //MM9DS1
    if(lsm.whoAmI()){
        pc.printf("LSM9DS1 is available\r\n");
        im920.send("MM-9DS1...OK",13);
    }else {
        pc.printf("LSM9DS1 is unavailable\r\n");
        im920.send("MM-9DS1...NG",13);
    }
    
    //LPS33HW
    if(lps33hw.whoAmI()){
        pc.printf("LPS33HW is available\r\n");
        im920.send("LPS33HW...OK",13);
    }else {
        pc.printf("LPS33HW is unavailable\r\n");
        im920.send("LPS33HW...NG",13);
    }
    
    //GPS
    gps.getgps();
    if(gps.result){
        pc.printf("GPS is available\r\n");
        im920.send("GPS...OK",9);
        im920.send(valueToChar(gps.longitude),7);
        im920.send(valueToChar(gps.latitude),7);
    }else{
        pc.printf("GPS is unavailable\r\n");
        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();
    }
}

char *valueToChar(float value){
    char buf[6];
    
    sprintf(buf,"%06.2f",value);
    
    return buf;
}

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