Diff: main.cpp

Revision:

5:03d31f0a4943

Parent:

3:1f93c4510fb1

Child:

7:1f2508ade0a3

diff -r 1f93c4510fb1 -r 03d31f0a4943 main.cpp
--- a/main.cpp	Wed Mar 06 10:34:09 2019 +0000
+++ b/main.cpp	Tue Mar 12 13:43:18 2019 +0000
@@ -1,5 +1,4 @@
 #include "mbed.h"
-#include <string.h>
 #include "LPS331_I2C.h"
 #include "LSM9DS1.h"
 #include "SDFileSystem.h"
@@ -11,7 +10,6 @@
 
 //ピン
 DigitalIn 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");
@@ -20,22 +18,18 @@
 LPS22HB lps33hw(p9,p10,LPS22HB_G_CHIP_ADDR);
 Timer waitTime;
 char sendData[256];
-//I2C i2c(p9,p10);
-//LPS22HB lps33hw(i2c, LPS22HB_G_CHIP_ADDR);
 PwmOut servo_1(p22), servo_2(p25);
 bool SDisAvailable=false;
+
 //関数
 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単位
-    int pretime=0;
-    //const int timeInterval=50;//ms単位
+    int pretime=0,pretime_2=0;
     float altitude,maxAltitude=0.0;//m単位
     float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持
     char receive[9]={};//受信した文字列
@@ -44,35 +38,84 @@
 
     FILE *fp;
     //サーボclose
-    servo_1.pulsewidth(0.89/1000.0);//ms/1000
+    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;
-        wait(5);
-        im920.send("Waiting_1",10);
+        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 == true){
+    while(flightpinAttached == false){//ランチャー取り付け中は取り外す
+        im920.poll();
+        memset(receive,'\0',sizeof(receive));
+        im920.recv(receive,8);
         flightpinAttached=flightpin;
-        wait(5);
-        im920.send("Waiting_2",10);
+        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("escap Waiting",14);
+    im920.send("escape Waiting",14);
     
-    while(strncmp(receive,"start",sizeof(receive))!=0){//無線での開始指令待ち txda tartと受信したら計測開始
+    while(strncmp(receive,"start",sizeof(receive))!=0){//無線での開始指令待ち txda startと受信したら計測開始
         im920.poll();
         memset(receive,'\0',sizeof(receive));
         im920.recv(receive,8);
@@ -86,19 +129,23 @@
         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>=2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始
+        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秒おきに経過時間送信
-            sprintf(sendData,"%010d,%06.2f",millis(),lps33hw.pressure());
-            im920.send(sendData,18);
+            lps33hw.get();
+            sprintf(sendData," %06.3fs, %06.2fPa",(float)millis()/1000,lps33hw.pressure());
+            im920.send(sendData,20);
             pretime = millis();
         }
     }
@@ -118,15 +165,7 @@
         //センサ・フライトピン読み取り 高度計測
         readValues(sequence);
         flightpinAttached=flightpin;
-        altitude=calcAltitude(lps331.getPressure(),lps331.getTemperature());
-        
-        pc.printf("%.2f\r\n",millis());
-        /*pc.printf("%f,%f\r\n", lps331.getPressure(), lps331.getTemperature());
-        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");*/
-        
+        altitude=calcAltitude(lps33hw.pressure(),lps33hw.temperature());
         
         if(maxAltitude<altitude){//最高高度更新
             maxAltitude=altitude;
@@ -135,13 +174,13 @@
         switch(sequence){
             case 0://発射待機中
                 //離床検知
-                if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている
+                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>=4.0){//加速度4.0G以上
-                    im920.send("LAUNCH ACC>4.0G",16);
+                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;
                 }
@@ -149,13 +188,15 @@
                 
             case 1://発射～減速機構展開
                 //減速機構展開
-                if(millis()-launchTime>=18*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下
-                    im920.send("Start speed reducing",21);
-                    reducerExpantionedTime=millis();
-                    //サーボopen
-                    servo_1.pulsewidth(1.28/1000);//ms/1000
-                    servo_2.pulsewidth(1.32/1000);
-                    sequence=2;
+                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;
                 
@@ -168,40 +209,73 @@
         }
         
         //SD書き込み time,temp,pres,temp_hw,pres_hw,accXYZ,gyroXYZ,magnXYZ,longitude,latitude
-        
         if(SDisAvailable){            
             if(sequence==1){//飛行中
-                fprintf(fp,"%d,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
-                millis(),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);
+                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,"%d,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n",
-                millis(),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);
+                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);
             }
-            
-            
-            //wait(0.5);
-            fclose(fp);
-            //wait(0.5);
-            //im920.send("File closed",12);
         }
-        //timer=timeInterval;
-        //wait((float)timeInterval/1000);
-        sprintf(sendData,"%1d:%06.2f,%010d",sequence,maxAltitude,millis());
-        im920.send(sendData,20);
+        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(sequence != 1){
-            waitTime.start();
-            waitTime.reset();
-            while(gps.result==false){//GPSが利用可能になるまでまつ
-                gps.getgps();
-                if(waitTime.read() > 10){
-                    waitTime.stop();
-                    break;
+        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);
             }
-            sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
-            im920.send(sendData,11);
+            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();
         }
     }
 }
@@ -213,46 +287,35 @@
     
     //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()){// who_am_i == b1 → false
-        pc.printf("LPS33HW is available\r\n");
+    if(!lps33hw.whoAmI()){
         im920.send("LPS33HW...OK",13);
     }else {
-        pc.printf("LPS33HW is unavailable\r\n");
         im920.send("LPS33HW...NG",13);
     }
     wait(1);
+    
     //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);
         SDisAvailable = false;
     }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");
+        fprintf(*file,"time,temperature,pressure,temperature_hw,pressure_hw,accX,accY,accZ,gyroX,gyroY,gyroZ,magnX,magnY,magnZ,longitude,latitude\r\n");
         SDisAvailable = true;
     }
     
@@ -260,12 +323,10 @@
     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);
+        sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
+        im920.send(sendData,11);
     }else{
-        pc.printf("GPS is unavailable\r\n");
         im920.send("GPS...NG",9);
     }
     
@@ -273,51 +334,43 @@
     //flightpin.output();
     
     //各種設定
-    lps331.setResolution(LPS331_I2C_PRESSURE_AVG_512, LPS331_I2C_TEMP_AVG_128);
+    /*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);
+    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);
+        sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
+        im920.send(sendData,11);
     }else{
-        pc.printf("GPS is unavailable\r\n");
         im920.send("GPS...NG",9);
     }
 }
@@ -334,14 +387,6 @@
     }
 }
 
-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