FTE-denshi
2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
Diff: main.cpp
- Revision:
- 7:1f2508ade0a3
- Parent:
- 5:03d31f0a4943
--- a/main.cpp Tue Mar 12 13:44:46 2019 +0000
+++ b/main.cpp Wed Jul 31 15:34:38 2019 +0000
@@ -4,39 +4,52 @@
#include "SDFileSystem.h"
#include "IM920.h"
#include "millis.h"
+#include "ADXL345_I2C.h"
#include "GPS.h"
-#include "LPS22HB.h"
-#include "stdio.h"
+#include <stdio.h>
+#include <string.h>
-//ピン
-DigitalIn flightpin(p12);
-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);
+//ピンとか
+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;
-char sendData[256];
-PwmOut servo_1(p22), servo_2(p25);
-bool SDisAvailable=false;
+
-//関数
+
+//function
void init(FILE **file);//諸々の初期化
void checkingSensors();
-void readValues(int sequence);//センサの読み取り
+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,maxAltitude=0.0;//m単位
+ 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();
-
- FILE *fp;
+
//サーボclose
servo_1.pulsewidth(1.05/1000.0);//ms/1000
servo_2.pulsewidth(1.94/1000.0);
@@ -44,128 +57,59 @@
init(&fp);//初期化
flightpinAttached=flightpin;
- while(flightpinAttached == false){
- im920.poll();
- memset(receive,'\0',sizeof(receive));
- im920.recv(receive,8);
- 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){
- im920.send("Waiting_1",10);
pretime=millis();
- }
- if(strncmp(receive,"nowait",sizeof(receive))==0){
- flightpinAttached=false;
- break;
- }
- if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
- im920.send("Retry initializing",19);
- init(&fp);
- wait(1);
- }
- if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
- im920.send("Mbed reset",11);
- NVIC_SystemReset();
- }
- }
- while(flightpinAttached == true){//組み立て中はつけたまま
- im920.poll();
- memset(receive,'\0',sizeof(receive));
- im920.recv(receive,8);
- flightpinAttached=flightpin;
- if(millis()-pretime>5000){
- im920.send("Waiting_2",10);
- pretime=millis();
- }
- if(strncmp(receive,"nowait",sizeof(receive))==0){
- flightpinAttached=false;
- break;
- }
- if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
- im920.send("Retry initializing",19);
- init(&fp);
- wait(1);
- }
- if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
- im920.send("Mbed reset",11);
- NVIC_SystemReset();
+ //センサ・フライトピン
+ 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");
+ }
}
}
- while(flightpinAttached == false){//ランチャー取り付け中は取り外す
- im920.poll();
- memset(receive,'\0',sizeof(receive));
- im920.recv(receive,8);
+
+ 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(millis()-pretime>5000){
- im920.send("Waiting_3",10);
- pretime=millis();
- }
- if(strncmp(receive,"nowait",sizeof(receive))==0){
- flightpinAttached=false;
- break;
- }
- if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
- im920.send("Retry initializing",19);
- init(&fp);
- wait(1);
- }
- if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
- im920.send("Mbed reset",11);
- NVIC_SystemReset();
- }
- }
- im920.send("escape Waiting",14);
-
- while(strncmp(receive,"start",sizeof(receive))!=0){//無線での開始指令待ち txda startと受信したら計測開始
- 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);
- wait(1);
- }
- 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);
+ 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秒おきに経過時間送信
- lps33hw.get();
- sprintf(sendData," %06.3fs, %06.2fPa",(float)millis()/1000,lps33hw.pressure());
- im920.send(sendData,20);
+ //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();
}
}
- im920.send("START",6);
+ imSend("RECORDEING START");
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();
- }
-
+ imRecv(receive);
+ mbedOperate(receive);
//センサ・フライトピン読み取り 高度計測
- readValues(sequence);
+ 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(lps33hw.pressure(),lps33hw.temperature());
+ altitude=calcAltitude(pressure,temperature);
if(maxAltitude<altitude){//最高高度更新
maxAltitude=altitude;
@@ -174,13 +118,13 @@
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);
+ 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以上かつフライトピンがぬけてない
- im920.send("LAUNCH ACC>3.9G",16);
+ imSend("LAUNCH ACC>3.9G");
launchTime=millis();
sequence=1;
}
@@ -188,9 +132,9 @@
case 1://発射～減速機構展開
//減速機構展開
- if(millis()-launchTime>=10*1000){
- if(millis()-launchTime>=15*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下
- im920.send("Start speed reducing",21);
+ 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
@@ -202,29 +146,63 @@
case 2://減速機構展開～着地
if(millis()-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過
- im920.send("LANDING",8);
+ imSend("LANDING");
sequence=3;
}
break;
}
- //SD書き込み time,temp,pres,temp_hw,pres_hw,accXYZ,gyroXYZ,magnXYZ,longitude,latitude
- if(SDisAvailable){
- if(sequence==1){//飛行中
- fprintf(fp,"%.3f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
- (float)millis()/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,"%.3f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n",
- (float)millis()/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(millis() - pretime_2 > 2000){//2秒おきにGPS送信
+ if(millis() - pretime_2 > 2000){//2秒おきに状態送信
sprintf(sendData," %1d: %06.2fm, %07.3fs",sequence,altitude,(float)millis()/1000);
- im920.send(sendData,22);
+ imSend(sendData);
pretime_2=millis();
}
- //地上局へのGPS情報送信
+ 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();
@@ -236,44 +214,8 @@
break;
}
}
- sprintf(sendData," %08.5fE, %08.5fN",gps.longitude,gps.latitude);
- im920.send(sendData,24);
- }
- pretime=millis();
- }
- }
- if(SDisAvailable){
- fclose(fp);
- im920.send("File closed",12);
- }else{
- im920.send("SD error",9);
- }
- sprintf(sendData," MaxAltitude: %06.2fm",maxAltitude);
- im920.send(sendData,23);
- while(1){
- //受信内容の初期化
- 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();
- }
-
- 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);
- im920.send(sendData,24);
+ sprintf(sendData,"%08.5fE,%08.5fN",gps.longitude,gps.latitude);
+ imSend(sendData);
}
pretime=millis();
}
@@ -287,106 +229,189 @@
//MMTXS03
if(lps331.isAvailable()) {
- im920.send("MM-TXS03...OK",14);
+ imSend("MM-TXS03,OK");
} else {
- im920.send("MM-TXS03...NG",14);
+ imSend("MM-TXS03,NG");
}
//MM9DS1
- lsm.begin();
- if(lsm.whoAmI()){
- im920.send("MM-9DS1...OK",13);
+ //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 {
- im920.send("MM-9DS1...NG",13);
- }
+ imSend("MM-9DS1,NG");
+ }*/
- //LPS33HW
- if(!lps33hw.whoAmI()){
- im920.send("LPS33HW...OK",13);
- }else {
- im920.send("LPS33HW...NG",13);
- }
+ //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) {
- im920.send("SD...NG",8);
+ imSend("SD,NG");
SDisAvailable = false;
}else{
- im920.send("SD...OK",8);
+ 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);
+ /*NVIC_SetPriority(UART3_IRQn,0);
gps.getgps();
if(gps.result){
- im920.send("GPS...OK",9);
+ imSend("GPS...OK");
sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
- im920.send(sendData,11);
+ imSend(sendData);
}else{
- im920.send("GPS...NG",9);
- }
+ imSend("GPS...NG");
+ }*/
//フライトピン
//flightpin.output();
//各種設定
- /*lps331.setResolution(LPS331_I2C_PRESSURE_AVG_512, LPS331_I2C_TEMP_AVG_128);
- lps331.setDataRate(LPS331_I2C_DATARATE_25HZ_T);
+ 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);*/
+ 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()) {
- im920.send("MM-TXS03...OK",14);
+ imSend("MM-TXS03,OK");
} else {
- im920.send("MM-TXS03...NG",14);
+ imSend("MM-TXS03,NG");
}
//MM9DS1
if(lsm.whoAmI()){
- im920.send("MM-9DS1...OK",13);
+ imSend("MM-9DS1,OK");
}else {
- im920.send("MM-9DS1...NG",13);
- }
-
- //LPS33HW
- if(!lps33hw.whoAmI()){
- im920.send("LPS33HW...OK",13);
- }else {
- im920.send("LPS33HW...NG",13);
+ imSend("MM-9DS1,NG");
}
+ //opening shock
+ if(accelerometer.getDeviceID()==0xE5) {
+ imSend("ADXL345,OK");
+ }else{
+ imSend("ADXL345,NG");
+ }
+
//GPS
- gps.getgps();
+ /*gps.getgps();
if(gps.result){
- im920.send("GPS...OK",9);
+ imSend("GPS...OK");
sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
- im920.send(sendData,11);
+ imSend(sendData);
}else{
- im920.send("GPS...NG",9);
- }
+ imSend("GPS...NG");
+ }*/
}
-void readValues(int sequence){
+void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings){
+ int accels[3];
+
lsm.readAccel();
lsm.readGyro();
lsm.readMag();
- lps331.getPressure();
- lps331.getTemperature();
- lps33hw.get();
- if(sequence!=1){
+ *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(1013.25/pres,1/5.257)-1)*(temp+273.15)/0.0065;
+ 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);
}
\ No newline at end of file