2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
main.cpp
- Committer:
- zeutel
- Date:
- 2020-09-25
- Revision:
- 8:b358cd848797
- Parent:
- 7:1f2508ade0a3
File content as of revision 8:b358cd848797:
#include "mbed.h" #include "LPS331_I2C.h" #include "LSM9DS1.h" #include "SDFileSystem.h" #include "IM920.h" #include "millis.h" #include "ADXL345_I2C.h" #include "GPS.h" #include <stdio.h> #include <string.h> //ピンとか Serial pc(USBTX, USBRX); DigitalIn flightpin(p12);//フライトピン LPS331_I2C lps331(p9, p10, LPS331_I2C_SA0_HIGH);//気温気圧 ADXL345_I2C accelerometer(p9, p10);//オープニングショック LSM9DS1 lsm(p9,p10);//9軸 IM920 im920(p28, p27, p29, p30);//無線 GPS gps(p13, p14);//GPS SDFileSystem sd(p5, p6, p7, p8, "sd");//SD PwmOut servo_1(p26), servo_2(p25);//サーボ Timer waitTime; //function void init(FILE **file);//諸々の初期化 void checkingSensors(); void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings);//センサの読み取り void imRecv(char *receive);//無線受信 float calcAltitude(float pres, float temp);//高度計算 void imSend(char *send); void mbedOperate(char *command); void sendDatas(float time,float temp,float pres,float ax,float ay,float az,float gx,float gy,float gz);//unity用data送信 //global char sendData[256]; FILE *fp; bool SDisAvailable=false; int main() { int sequence=0;//機体の状態の推移 int pretime=0,pretime_2=0; float altitude=-1000.0,maxAltitude=-1000.0;//m単位 float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持 char receive[9]={};//受信した文字列 bool flightpinAttached=false;//フライトピンが付いているかどうか float temperature,pressure; float accelReadings[3] = {0, 0, 0};//x,y,z millisStart(); //サーボclose servo_1.pulsewidth(1.05/1000.0);//ms/1000 servo_2.pulsewidth(1.94/1000.0); init(&fp);//初期化 flightpinAttached=flightpin; while(strncmp(receive,"escape",sizeof(receive))!=0){//準備中 imRecv(receive); mbedOperate(receive); readValues(sequence,&temperature,&pressure,accelReadings); sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz); if(millis()-pretime>5000){ pretime=millis(); //センサ・フライトピン flightpinAttached=flightpin; //altitude=calcAltitude(pressure,temperature); //sprintf(sendData,"time=%06.3fs, alt=%.2fm, absAcc=%.2fm/s^2",(float)millis()/1000,altitude,sqrt(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az)); //imSend(sendData); imSend("Waiting: command 'escape' to escape waiting"); if(flightpinAttached==false){ imSend("flightpin isn't attached"); } } } imSend("Escape Waiting"); while(strncmp(receive,"record",sizeof(receive))!=0){//開始指令待ち imRecv(receive); mbedOperate(receive); //加速度・フライトピン読み取り readValues(sequence,&temperature,&pressure,accelReadings); sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz); flightpinAttached=flightpin; if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0*2.0 && flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始 imSend("Forced start"); launchTime=millis(); sequence=1; break; } //地上局への送信 if(millis() - pretime > 5000){//5秒おきに経過時間送信 //altitude=calcAltitude(pressure,temperature); //sprintf(sendData,"time=%06.3fs, alt=%.2fm, absAcc=%.2fm/s^2",(float)millis()/1000,altitude,sqrt(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az)); //imSend(sendData); imSend("Start Waiting: command 'record' to start recording"); pretime = millis(); } } imSend("RECORDEING START"); while(strncmp(receive,"end",sizeof(receive))!=0&&sequence!=3) {//endと受信するまたは着地したら終了 imRecv(receive); mbedOperate(receive); //センサ・フライトピン読み取り 高度計測 readValues(sequence,&temperature,&pressure,accelReadings); sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz); flightpinAttached=flightpin; altitude=calcAltitude(pressure,temperature); if(maxAltitude<altitude){//最高高度更新 maxAltitude=altitude; } switch(sequence){ case 0://発射待機中 //離床検知 if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0*2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている imSend("LAUNCH ACC>2.0G"); launchTime=millis(); sequence=1; } if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=3.9*3.9 &&flightpinAttached==true){//加速度4.0G以上かつフライトピンがぬけてない imSend("LAUNCH ACC>3.9G"); launchTime=millis(); sequence=1; } break; case 1://発射~減速機構展開 //減速機構展開 if(millis()-launchTime>=13*1000){ if(millis()-launchTime>=15*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下 imSend("Start speed reducing"); reducerExpantionedTime=millis(); //サーボopen servo_1.pulsewidth(1.90/1000);//ms/1000 servo_2.pulsewidth(1.32/1000); sequence=2; } } break; case 2://減速機構展開~着地 if(millis()-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過 imSend("LANDING"); sequence=3; } break; } if(millis() - pretime_2 > 2000){//2秒おきに状態送信 sprintf(sendData," %1d: %06.2fm, %07.3fs",sequence,altitude,(float)millis()/1000); imSend(sendData); pretime_2=millis(); } if(sequence==1){ //SD書き込み if(SDisAvailable){ fprintf(fp,"%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", (float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz); } }else{ //SD書き込み if(SDisAvailable){ fprintf(fp,"%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n", (float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz,gps.longitude,gps.latitude); } //地上局へのGPS情報送信 /*if(millis() - pretime > 5000){//5秒おきにGPS送信 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); imSend(sendData); }*/ pretime=millis(); } } //着地後 if(SDisAvailable){ fclose(fp); imSend("File closed"); }else{ imSend("SD error"); } sprintf(sendData,"MaxAltitude:%06.2fm",maxAltitude); imSend(sendData); while(1){ imRecv(receive); mbedOperate(receive); 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); imSend(sendData); } pretime=millis(); } } } void init(FILE **file){ //im920 im920.init(); wait(1); //MMTXS03 if(lps331.isAvailable()) { imSend("MM-TXS03,OK"); } else { imSend("MM-TXS03,NG"); } //MM9DS1 //lsm.begin(); uint16_t WhoIsLsm = lsm.begin(); if(WhoIsLsm==0x683D){ imSend("MM-9DS1,OK"); }else{ imSend("MM-9DS1,NG"); } /*if(lsm.whoAmI()){ imSend("MM-9DS1,OK"); }else { imSend("MM-9DS1,NG"); }*/ //opening shock if(accelerometer.getDeviceID()==0xE5) { imSend("ADXL345,OK"); }else{ imSend("ADXL345,NG"); } wait(1); //SD *file = fopen("/sd/data.csv", "w"); if(*file == NULL) { imSend("SD,NG"); SDisAvailable = false; }else{ imSend("SD,OK"); fprintf(*file,"time,temperature,pressure,temperature_hw,pressure_hw,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){ imSend("GPS...OK"); sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude); imSend(sendData); }else{ imSend("GPS...NG"); }*/ //フライトピン //flightpin.output(); //各種設定 lps331.setResolution(LPS331_I2C_PRESSURE_AVG_512, LPS331_I2C_TEMP_AVG_128); lps331.setDataRate(LPS331_I2C_DATARATE_7HZ); lps331.setActive(true); lsm.setAccelODR(lsm.A_ODR_952); lsm.setGyroODR(lsm.G_ODR_952_BW_100); lsm.setMagODR(lsm.M_ODR_80); // These are here to test whether any of the initialization fails. It will print the failure /*if (accelerometer.setPowerControl(0x00)){ pc.printf("didn't intitialize power control\r\n"); } //Full resolution, +/-16g, 4mg/LSB. wait(.001); if(accelerometer.setDataFormatControl(0x0B)){ pc.printf("didn't set data format\r\n"); } wait(.001);*/ //3.2kHz data rate. if(accelerometer.setDataRate(ADXL345_200HZ)){ pc.printf("didn't set data rate\r\n"); } wait(.001); //分解能 accelerometer.setOpticalResolution(0x0B); //測定モード if(accelerometer.setPowerControl(MeasurementMode)) { pc.printf("didn't set the power control to measurement\r\n"); } } void checkingSensors(){ //MMTXS03 if(lps331.isAvailable()) { imSend("MM-TXS03,OK"); } else { imSend("MM-TXS03,NG"); } //MM9DS1 if(lsm.whoAmI()){ imSend("MM-9DS1,OK"); }else { imSend("MM-9DS1,NG"); } //opening shock if(accelerometer.getDeviceID()==0xE5) { imSend("ADXL345,OK"); }else{ imSend("ADXL345,NG"); } //GPS /*gps.getgps(); if(gps.result){ imSend("GPS...OK"); sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude); imSend(sendData); }else{ imSend("GPS...NG"); }*/ } void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings){ int accels[3]; lsm.readAccel(); lsm.readGyro(); lsm.readMag(); *pressure=lps331.getPressure(); *temperature=lps331.getTemperature(); /*if(sequence!=1){ gps.getgps(); }*/ accelerometer.getOutput(accels); for(int i=0;i<3;i++){ openingShockReadings[i]=(int16_t)accels[i];//*0.0392266; } } float calcAltitude(float pres, float temp){ return (pow(1015.5/pres,1/5.257)-1)*(temp+273.15)/0.0065; } void imRecv(char *receive){ im920.poll(); memset(receive,'\0',sizeof(receive)); im920.recv(receive,8); pc.printf(receive); //camera用コマンド認識 char *camStr=strtok(receive,":");//cam:startというようにコマンド if(strncmp(camStr,"cam",sizeof(camStr))==0){ imSend(receive); } } void imSend(char *send){ im920.send(send,strlen(send)+1); pc.printf(send); pc.printf("\r\n"); } void mbedOperate(char *command){ if(strncmp(command,"reset",sizeof(command))==0){ imSend("Mbed reset"); NVIC_SystemReset(); } if(strncmp(command,"retry",sizeof(command))==0){ imSend("Retry initializing"); init(&fp); wait(1); } if(strncmp(command,"check",sizeof(command))==0){ imSend("Check"); checkingSensors(); } } void sendDatas(float time, float temp, float pres, float ax, float ay, float az, float gx, float gy, float gz){ sprintf(sendData,"data1,%.3f,%.3f,%.3f",time,temp,pres); imSend(sendData); sprintf(sendData,"data2,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f",ax,ay,az,gx,gy,gz); imSend(sendData); }