FTE-denshi
2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
Diff: main.cpp
- Revision:
- 4:ad2faabb7995
- Parent:
- 0:cebf1f73ffd5
--- 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);