2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
Diff: main.cpp
- Revision:
- 4:ad2faabb7995
- Parent:
- 0:cebf1f73ffd5
diff -r c2829a442621 -r ad2faabb7995 main.cpp --- a/main.cpp Sat Mar 02 15:20:47 2019 +0000 +++ b/main.cpp Wed Mar 06 01:18:13 2019 +0000 @@ -8,7 +8,7 @@ #include "LPS22HB.h" //ピン -DigitalInOut flightpin(p12); +DigitalIn flightpin(p12); Serial pc(USBTX, USBRX); LPS331_I2C lps331(p9, p10, LPS331_I2C_SA0_HIGH); LSM9DS1 lsm(p9,p10); @@ -36,20 +36,25 @@ char receive[9]={};//受信した文字列 bool flightpinAttached=false;//フライトピンが付いているかどうか FILE *fp; + servo_1.period(0.020); + servo_2.period(0.020); + init(&fp);//初期化 - init(&fp);//初期化 + //flightpin.mode(PullNone); //サーボclose - //servo_1.pulsewidth(0.76/1000);//ms/1000 - //servo_2.pulsewidth(1.94/1000); + servo_1.pulsewidth(0.00100); + servo_2.pulsewidth(0.00194); + pc.printf("servo close"); - 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); //加速度・フライトピン読み取り lsm.readAccel(); flightpinAttached=flightpin; + pc.printf("%d\r\n",flightpin.read()); if(strncmp(receive,"check",sizeof(receive))==0){//checkと受信したらセンサーの状態確認 主にGPS im920.send("Check",6); checkingSensors(); @@ -62,7 +67,7 @@ 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>=4.0&&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始 im920.send("Forced start",13); break; } @@ -82,10 +87,10 @@ //センサ・フライトピン読み取り 高度計測 readValues(sequence); - flightpinAttached=flightpin; + //flightpinAttached=flightpin; altitude=calcAltitude(lps331.pressure,lps331.temperature); - pc.printf("%.2f\r\n",(float)timer/1000); + //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); @@ -104,7 +109,7 @@ 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>=4.0&&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている im920.send("LAUNCH ACC>2.0G",16); launchTime=timer; sequence=1; @@ -117,8 +122,8 @@ im920.send("Start speed reducing",21); reducerExpantionedTime=timer; //サーボopen - //servo_1.pulsewidth(1.28/1000);//ms/1000 - //servo_2.pulsewidth(1.32/1000); + servo_1.pulsewidth(0.00128);//ms/1000 + servo_2.pulsewidth(0.00132); sequence=2; } break; @@ -132,21 +137,26 @@ } //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); + if(fp!=NULL){ + 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); + if(fp!=NULL){ + fclose(fp); + wait(0.5); + im920.send("File closed",12); + }else{ + im920.send("SD error",9); + } im920.send("MaxAltitude=",13); im920.send(valueToChar(maxAltitude),7); @@ -165,7 +175,7 @@ wait(1); //MMTXS03 - if(lps331.isAvailable()) { + if(lps331.isAvailable()==true) { pc.printf("LPS331 is available\r\n"); im920.send("MM-TXS03...OK",14); } else { @@ -175,7 +185,7 @@ //MM9DS1 lsm.begin(); - if(lsm.whoAmI()){ + if(lsm.whoAmI()==true){ pc.printf("LSM9DS1 is available\r\n"); im920.send("MM-9DS1...OK",13); }else { @@ -184,7 +194,7 @@ } //LPS33HW - if(lps33hw.whoAmI()){ + if(lps33hw.whoAmI()==true){ pc.printf("LPS33HW is available\r\n"); im920.send("LPS33HW...OK",13); }else { @@ -193,9 +203,9 @@ } //SD - if(*file!=NULL){ + /*if(*file!=NULL){ fclose(*file); - } + }*/ *file = fopen("/sd/data.csv", "w"); if(*file == NULL) { pc.printf("Could not open file for write\r\n"); @@ -219,13 +229,10 @@ im920.send("GPS...NG",9); } - //フライトピン - 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); + lps331.setActive(true);*/ lsm.setAccelODR(lsm.A_ODR_952); lsm.setGyroODR(lsm.G_ODR_952_BW_100); lsm.setMagODR(lsm.M_ODR_80);