2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
Diff: main.cpp
- Revision:
- 0:cebf1f73ffd5
- Child:
- 2:cad76b5be4ba
- Child:
- 4:ad2faabb7995
diff -r 000000000000 -r cebf1f73ffd5 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Mar 02 15:15:47 2019 +0000 @@ -0,0 +1,297 @@ +#include "mbed.h" +#include <string.h> +#include "LPS331_I2C.h" +#include "LSM9DS1.h" +#include "SDFileSystem.h" +#include "IM920.h" +#include "GPS.h" +#include "LPS22HB.h" + +//ピン +DigitalInOut 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); +//I2C i2c(p9,p10); +//LPS22HB lps33hw(i2c, LPS22HB_G_CHIP_ADDR); +PwmOut servo_1(p22), servo_2(p25); + +//関数 +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単位 + const int timeInterval=50;//ms単位 + float altitude,maxAltitude=0.0;//m単位 + float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持 + char receive[9]={};//受信した文字列 + bool flightpinAttached=false;//フライトピンが付いているかどうか + FILE *fp; + + init(&fp);//初期化 + + //サーボclose + //servo_1.pulsewidth(0.76/1000);//ms/1000 + //servo_2.pulsewidth(1.94/1000); + + 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>=4.0/*&&flightpinAttached==false*/){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始 + im920.send("Forced start",13); + break; + } + } + 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.pressure,lps331.temperature); + + 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); + pc.printf("magnX=%.3f / magnY=%.3f / magnZ=%.3f\r\n",lsm.mx,lsm.my,lsm.mz); + pc.printf("\r\n");*/ + + //地上局への送信 + if(timer%1000==0){//1秒おきに経過時間送信 + im920.send(valueToChar((float)timer/1000),7); + } + + if(maxAltitude<altitude){//最高高度更新 + maxAltitude=altitude; + } + + switch(sequence){ + case 0://発射待機中 + //離床検知 + 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; + } + break; + + case 1://発射~減速機構展開 + //減速機構展開 + if(timer-launchTime>=18*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下 + im920.send("Start speed reducing",21); + reducerExpantionedTime=timer; + //サーボopen + //servo_1.pulsewidth(1.28/1000);//ms/1000 + //servo_2.pulsewidth(1.32/1000); + sequence=2; + } + break; + + case 2://減速機構展開~着地 + if(timer-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(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); + 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){ + //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()){ + 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); + } + + //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); + }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"); + } + + //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; +} \ No newline at end of file