Diff: main.cpp

Revision:

0:cebf1f73ffd5

Child:

2:cad76b5be4ba

Child:

4:ad2faabb7995

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Mar 02 15:15:47 2019 +0000
@@ -0,0 +1,297 @@
+#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;
+}
\ No newline at end of file