2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
Diff: main.cpp
- Revision:
- 3:1f93c4510fb1
- Parent:
- 2:cad76b5be4ba
- Child:
- 5:03d31f0a4943
diff -r cad76b5be4ba -r 1f93c4510fb1 main.cpp --- a/main.cpp Wed Mar 06 05:35:46 2019 +0000 +++ b/main.cpp Wed Mar 06 10:34:09 2019 +0000 @@ -4,8 +4,10 @@ #include "LSM9DS1.h" #include "SDFileSystem.h" #include "IM920.h" +#include "millis.h" #include "GPS.h" #include "LPS22HB.h" +#include "stdio.h" //ピン DigitalIn flightpin(p12); @@ -16,10 +18,12 @@ IM920 im920(p28, p27, p29, p30); GPS gps(p13, p14); 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(); @@ -29,19 +33,44 @@ int main() { int sequence=0;//機体の状態の推移 - int timer=0;//ms単位 - const int timeInterval=50;//ms単位 + //int timer=0;//ms単位 + int pretime=0; + //const int timeInterval=50;//ms単位 float altitude,maxAltitude=0.0;//m単位 float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持 char receive[9]={};//受信した文字列 bool flightpinAttached=false;//フライトピンが付いているかどうか + millisStart(); + FILE *fp; + //サーボclose + servo_1.pulsewidth(0.89/1000.0);//ms/1000 + servo_2.pulsewidth(1.94/1000.0); init(&fp);//初期化 + flightpinAttached=flightpin; - //サーボclose - servo_1.pulsewidth(0.89/1000);//ms/1000 - servo_2.pulsewidth(1.94/1000); + + + + while(flightpinAttached == false){ + im920.poll(); + memset(receive,'\0',sizeof(receive)); + im920.recv(receive,8); + flightpinAttached=flightpin; + wait(5); + im920.send("Waiting_1",10); + if(strncmp(receive,"nowait",sizeof(receive))==0){ + flightpinAttached=false; + break; + } + } + while(flightpinAttached == true){ + flightpinAttached=flightpin; + wait(5); + im920.send("Waiting_2",10); + } + im920.send("escap Waiting",14); while(strncmp(receive,"start",sizeof(receive))!=0){//無線での開始指令待ち txda tartと受信したら計測開始 im920.poll(); @@ -62,10 +91,16 @@ 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以上かつフライトピンがぬけていたらコマンドがなくても開始 + if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始 im920.send("Forced start",13); break; } + //地上局への送信 + if(millis() - pretime > 5000){//5秒おきに経過時間送信 + sprintf(sendData,"%010d,%06.2f",millis(),lps33hw.pressure()); + im920.send(sendData,18); + pretime = millis(); + } } im920.send("START",6); @@ -85,17 +120,13 @@ flightpinAttached=flightpin; altitude=calcAltitude(lps331.getPressure(),lps331.getTemperature()); - pc.printf("%.2f\r\n",(float)timer/1000); + 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");*/ - //地上局への送信 - if(timer%5000==0){//5秒おきに経過時間送信 - im920.send(valueToChar((float)timer/1000),7); - } if(maxAltitude<altitude){//最高高度更新 maxAltitude=altitude; @@ -104,18 +135,23 @@ switch(sequence){ case 0://発射待機中 //離床検知 - if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=4.0/*&&flightpinAttached==false*/){//加速度2.0G以上かつフライトピンがぬけている + if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている im920.send("LAUNCH ACC>2.0G",16); - launchTime=timer; + 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); + launchTime=millis(); sequence=1; } break; case 1://発射~減速機構展開 //減速機構展開 - if(timer-launchTime>=18*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下 + if(millis()-launchTime>=18*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下 im920.send("Start speed reducing",21); - reducerExpantionedTime=timer; + reducerExpantionedTime=millis(); //サーボopen servo_1.pulsewidth(1.28/1000);//ms/1000 servo_2.pulsewidth(1.32/1000); @@ -124,7 +160,7 @@ break; case 2://減速機構展開~着地 - if(timer-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過 + if(millis()-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過 im920.send("LANDING",8); sequence=3; } @@ -132,31 +168,42 @@ } //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.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,"%.2f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n", - (float)timer/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(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); + }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); + } + + + //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); - timer+=timeInterval; - wait((float)timeInterval/1000); + //地上局へのGPS情報送信 + if(sequence != 1){ + waitTime.start(); + waitTime.reset(); + while(gps.result==false){//GPSが利用可能になるまでまつ + gps.getgps(); + if(waitTime.read() > 10){ + waitTime.stop(); + break; + } + } + sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude); + im920.send(sendData,11); + } } - 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){ @@ -192,7 +239,7 @@ pc.printf("LPS33HW is unavailable\r\n"); im920.send("LPS33HW...NG",13); } - + wait(1); //SD if(*file!=NULL){ fclose(*file); @@ -201,10 +248,12 @@ 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"); + SDisAvailable = true; } //GPS優先度