2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
Diff: main.cpp
- Revision:
- 5:03d31f0a4943
- Parent:
- 3:1f93c4510fb1
- Child:
- 7:1f2508ade0a3
--- 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