FTE-denshi
2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
Diff: main.cpp
- Revision:
- 3:1f93c4510fb1
- Parent:
- 2:cad76b5be4ba
- Child:
- 5:03d31f0a4943
--- a/main.cpp Wed Mar 06 05:35:46 2019 +0000
+++ b/main.cpp Wed Mar 06 10:34:09 2019 +0000
@@ -4,8 +4,10 @@
#include "LSM9DS1.h"
#include "SDFileSystem.h"
#include "IM920.h"
+#include "millis.h"
#include "GPS.h"
#include "LPS22HB.h"
+#include "stdio.h"
//ピン
DigitalIn flightpin(p12);
@@ -16,10 +18,12 @@
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();
@@ -29,19 +33,44 @@
int main() {
int sequence=0;//機体の状態の推移
- int timer=0;//ms単位
- const int timeInterval=50;//ms単位
+ //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;
- //サーボclose
- servo_1.pulsewidth(0.89/1000);//ms/1000
- servo_2.pulsewidth(1.94/1000);
+
+
+
+ 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();
@@ -62,10 +91,16 @@
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>=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);
@@ -85,17 +120,13 @@
flightpinAttached=flightpin;
altitude=calcAltitude(lps331.getPressure(),lps331.getTemperature());
- pc.printf("%.2f\r\n",(float)timer/1000);
+ 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(timer%5000==0){//5秒おきに経過時間送信
- im920.send(valueToChar((float)timer/1000),7);
- }
if(maxAltitude<altitude){//最高高度更新
maxAltitude=altitude;
@@ -104,18 +135,23 @@
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>=2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている
im920.send("LAUNCH ACC>2.0G",16);
- launchTime=timer;
+ 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(timer-launchTime>=18*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下
+ if(millis()-launchTime>=18*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下
im920.send("Start speed reducing",21);
- reducerExpantionedTime=timer;
+ reducerExpantionedTime=millis();
//サーボopen
servo_1.pulsewidth(1.28/1000);//ms/1000
servo_2.pulsewidth(1.32/1000);
@@ -124,7 +160,7 @@
break;
case 2://減速機構展開～着地
- if(timer-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過
+ if(millis()-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過
im920.send("LANDING",8);
sequence=3;
}
@@ -132,31 +168,42 @@
}
//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.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,"%.2f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n",
- (float)timer/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);
+
+ 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);
- timer+=timeInterval;
- wait((float)timeInterval/1000);
+ //地上局への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);
+ }
}
- 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){
@@ -192,7 +239,7 @@
pc.printf("LPS33HW is unavailable\r\n");
im920.send("LPS33HW...NG",13);
}
-
+ wait(1);
//SD
if(*file!=NULL){
fclose(*file);
@@ -201,10 +248,12 @@
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優先度