File content as of revision 7:1f2508ade0a3:

#include "mbed.h"
#include "LPS331_I2C.h"
#include "LSM9DS1.h"
#include "SDFileSystem.h"
#include "IM920.h"
#include "millis.h"
#include "ADXL345_I2C.h"
#include "GPS.h"
#include <stdio.h>
#include <string.h>

//ピンとか
Serial pc(USBTX, USBRX);
DigitalIn flightpin(p12);//フライトピン
LPS331_I2C lps331(p9, p10, LPS331_I2C_SA0_HIGH);//気温気圧
 ADXL345_I2C accelerometer(p9, p10);//オープニングショック
LSM9DS1 lsm(p9,p10);//9軸
IM920 im920(p28, p27, p29, p30);//無線
GPS gps(p13, p14);//GPS
SDFileSystem sd(p5, p6, p7, p8, "sd");//SD
PwmOut servo_1(p26), servo_2(p25);//サーボ
Timer waitTime;



//function
void init(FILE **file);//諸々の初期化
void checkingSensors();
void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings);//センサの読み取り
void imRecv(char *receive);//無線受信
float calcAltitude(float pres, float temp);//高度計算
void imSend(char *send);
void mbedOperate(char *command);
void sendDatas(float time,float temp,float pres,float ax,float ay,float az,float gx,float gy,float gz);//unity用data送信

//global
char sendData[256];
FILE *fp;
bool SDisAvailable=false;

int main() {
    int sequence=0;//機体の状態の推移
    int pretime=0,pretime_2=0;
    float altitude=-1000.0,maxAltitude=-1000.0;//m単位
    float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持
    char receive[9]={};//受信した文字列
    bool flightpinAttached=false;//フライトピンが付いているかどうか
    float temperature,pressure;
    float accelReadings[3] = {0, 0, 0};//x,y,z
    
    millisStart();
    
    //サーボclose
    servo_1.pulsewidth(1.05/1000.0);//ms/1000
    servo_2.pulsewidth(1.94/1000.0);
    
    init(&fp);//初期化
    flightpinAttached=flightpin;
    
    while(strncmp(receive,"escape",sizeof(receive))!=0){//準備中
        imRecv(receive);
        mbedOperate(receive);
        readValues(sequence,&temperature,&pressure,accelReadings);
        sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz);
        if(millis()-pretime>5000){
            pretime=millis();
            //センサ・フライトピン
            flightpinAttached=flightpin;
            //altitude=calcAltitude(pressure,temperature);
            //sprintf(sendData,"time=%06.3fs, alt=%.2fm, absAcc=%.2fm/s^2",(float)millis()/1000,altitude,sqrt(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az));
            //imSend(sendData);
            imSend("Waiting: command 'escape' to escape waiting");
            if(flightpinAttached==false){
                imSend("flightpin isn't attached");
            }
        }
    }
    
    imSend("Escape Waiting");
    
    while(strncmp(receive,"record",sizeof(receive))!=0){//開始指令待ち
        imRecv(receive);
        mbedOperate(receive);
        //加速度・フライトピン読み取り
        readValues(sequence,&temperature,&pressure,accelReadings);
        sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz);
        flightpinAttached=flightpin;
        if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0*2.0 && flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始
            imSend("Forced start");
            launchTime=millis();
            sequence=1;
            break;
        }
        //地上局への送信
        if(millis() - pretime > 5000){//5秒おきに経過時間送信
            //altitude=calcAltitude(pressure,temperature);
            //sprintf(sendData,"time=%06.3fs, alt=%.2fm, absAcc=%.2fm/s^2",(float)millis()/1000,altitude,sqrt(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az));
            //imSend(sendData);
            imSend("Start Waiting: command 'record' to start recording");
            pretime = millis();
        }
    }
    imSend("RECORDEING START");
        
    while(strncmp(receive,"end",sizeof(receive))!=0&&sequence!=3) {//endと受信するまたは着地したら終了
        imRecv(receive);
        mbedOperate(receive);
        //センサ・フライトピン読み取り 高度計測
        readValues(sequence,&temperature,&pressure,accelReadings);
        sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz);
        flightpinAttached=flightpin;
        altitude=calcAltitude(pressure,temperature);
        
        if(maxAltitude<altitude){//最高高度更新
            maxAltitude=altitude;
        }
        
        switch(sequence){
            case 0://発射待機中
                //離床検知
                if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0*2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている
                    imSend("LAUNCH ACC>2.0G");
                    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以上かつフライトピンがぬけてない
                    imSend("LAUNCH ACC>3.9G");
                    launchTime=millis();
                    sequence=1;
                }
                break;
                
            case 1://発射～減速機構展開
                //減速機構展開
                if(millis()-launchTime>=13*1000){
                    if(millis()-launchTime>=15*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下 
                        imSend("Start speed reducing");
                        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秒経過
                    imSend("LANDING");
                    sequence=3;
                }
                break;
        }
        
        if(millis() - pretime_2 > 2000){//2秒おきに状態送信
            sprintf(sendData," %1d: %06.2fm, %07.3fs",sequence,altitude,(float)millis()/1000);
            imSend(sendData);
            pretime_2=millis();
        }
        
        if(sequence==1){
            //SD書き込み
            if(SDisAvailable){
                fprintf(fp,"%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
                (float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz);
            }
        }else{
            //SD書き込み
            if(SDisAvailable){
                fprintf(fp,"%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n",
                (float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz,gps.longitude,gps.latitude);
            }
            
            //地上局へのGPS情報送信
            /*if(millis() - pretime > 5000){//5秒おきにGPS送信
            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);
                imSend(sendData);
            }*/
            pretime=millis();
        }
    }
    
    //着地後
    if(SDisAvailable){
        fclose(fp);
        imSend("File closed");
    }else{
        imSend("SD error");
    }
    
    sprintf(sendData,"MaxAltitude:%06.2fm",maxAltitude);
    imSend(sendData);
    
    while(1){
        imRecv(receive);
        mbedOperate(receive);
        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);
                imSend(sendData);
            }
            pretime=millis();
        }
    }
}

void init(FILE **file){
     //im920
    im920.init();
    wait(1);
    
    //MMTXS03
    if(lps331.isAvailable()) {
        imSend("MM-TXS03,OK");
    } else {
        imSend("MM-TXS03,NG");
    }
    
    //MM9DS1
    //lsm.begin();
    uint16_t WhoIsLsm = lsm.begin();
    if(WhoIsLsm==0x683D){
        imSend("MM-9DS1,OK");
    }else{
        imSend("MM-9DS1,NG");
    }
        /*if(lsm.whoAmI()){
        imSend("MM-9DS1,OK");
    }else {
        imSend("MM-9DS1,NG");
    }*/
    
    //opening shock
    if(accelerometer.getDeviceID()==0xE5) {
        imSend("ADXL345,OK"); 
    }else{
        imSend("ADXL345,NG"); 
    } 
    
    wait(1);
    
    //SD
    *file = fopen("/sd/data.csv", "w");
    if(*file == NULL) {
        imSend("SD,NG");
        SDisAvailable = false;
    }else{
        imSend("SD,OK");
        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){
        imSend("GPS...OK");
        sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
        imSend(sendData);
    }else{
        imSend("GPS...NG");
    }*/
    
    //フライトピン
    //flightpin.output();
    
    //各種設定
    lps331.setResolution(LPS331_I2C_PRESSURE_AVG_512, LPS331_I2C_TEMP_AVG_128);
    lps331.setDataRate(LPS331_I2C_DATARATE_7HZ);
    lps331.setActive(true);
    lsm.setAccelODR(lsm.A_ODR_952);
    lsm.setGyroODR(lsm.G_ODR_952_BW_100);
    lsm.setMagODR(lsm.M_ODR_80);
    
    // These are here to test whether any of the initialization fails. It will print the failure
     /*if (accelerometer.setPowerControl(0x00)){
        pc.printf("didn't intitialize power control\r\n");
     }
     //Full resolution, +/-16g, 4mg/LSB.
     wait(.001);
     
     if(accelerometer.setDataFormatControl(0x0B)){
        pc.printf("didn't set data format\r\n");
     }
     wait(.001);*/
     
     //3.2kHz data rate.
     if(accelerometer.setDataRate(ADXL345_200HZ)){
        pc.printf("didn't set data rate\r\n");
     }
     wait(.001);
     
     //分解能
     accelerometer.setOpticalResolution(0x0B);
     
     //測定モード
     if(accelerometer.setPowerControl(MeasurementMode)) {
        pc.printf("didn't set the power control to measurement\r\n"); 
     } 
     
     
}

void checkingSensors(){
    //MMTXS03
    if(lps331.isAvailable()) {
        imSend("MM-TXS03,OK");
    } else {
        imSend("MM-TXS03,NG");
    }
    
    //MM9DS1
    if(lsm.whoAmI()){
        imSend("MM-9DS1,OK");
    }else {
        imSend("MM-9DS1,NG");
    }
    
    //opening shock
    if(accelerometer.getDeviceID()==0xE5) {
        imSend("ADXL345,OK"); 
    }else{
        imSend("ADXL345,NG"); 
    } 
    
    //GPS
    /*gps.getgps();
    if(gps.result){
        imSend("GPS...OK");
        sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
        imSend(sendData);
    }else{
        imSend("GPS...NG");
    }*/
}

void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings){
    int accels[3];
    
    lsm.readAccel();
    lsm.readGyro();
    lsm.readMag();
    *pressure=lps331.getPressure();
    *temperature=lps331.getTemperature();
    /*if(sequence!=1){
        gps.getgps();
    }*/
    accelerometer.getOutput(accels);
    for(int i=0;i<3;i++){
        openingShockReadings[i]=(int16_t)accels[i];//*0.0392266;
    }
}

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

void imRecv(char *receive){
    im920.poll();
    memset(receive,'\0',sizeof(receive));
    im920.recv(receive,8);
    pc.printf(receive);
    //camera用コマンド認識
    char *camStr=strtok(receive,":");//cam:startというようにコマンド
    if(strncmp(camStr,"cam",sizeof(camStr))==0){
        imSend(receive);
    }
}

void imSend(char *send){
    im920.send(send,strlen(send)+1);
    pc.printf(send);
    pc.printf("\r\n");
}

void mbedOperate(char *command){
    if(strncmp(command,"reset",sizeof(command))==0){
        imSend("Mbed reset");
        NVIC_SystemReset();
    }
    
    if(strncmp(command,"retry",sizeof(command))==0){
        imSend("Retry initializing");
        init(&fp);
        wait(1);
    }
    
    if(strncmp(command,"check",sizeof(command))==0){
        imSend("Check");
        checkingSensors();
    }
}

void sendDatas(float time, float temp, float pres, float ax, float ay, float az, float gx, float gy, float gz){
        sprintf(sendData,"data1,%.3f,%.3f,%.3f",time,temp,pres);
        imSend(sendData);
        sprintf(sendData,"data2,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f",ax,ay,az,gx,gy,gz);
        imSend(sendData);
}