2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
main.cpp
- Committer:
- Nozaryo
- Date:
- 2019-03-06
- Revision:
- 3:1f93c4510fb1
- Parent:
- 2:cad76b5be4ba
- Child:
- 5:03d31f0a4943
File content as of revision 3:1f93c4510fb1:
#include "mbed.h" #include <string.h> #include "LPS331_I2C.h" #include "LSM9DS1.h" #include "SDFileSystem.h" #include "IM920.h" #include "millis.h" #include "GPS.h" #include "LPS22HB.h" #include "stdio.h" //ピン 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"); 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(); 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単位 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; 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(); 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>=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); 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.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");*/ if(maxAltitude<altitude){//最高高度更新 maxAltitude=altitude; } switch(sequence){ case 0://発射待機中 //離床検知 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=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(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; } break; case 2://減速機構展開~着地 if(millis()-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(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); //地上局への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); } } } 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()){// who_am_i == b1 → false 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); } 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"); SDisAvailable = true; } //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; }