FTE-denshi / Mbed 2 deprecated Izu_2019

2019伊豆のコード

Dependencies:   mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis

main.cpp@5:03d31f0a4943, 2019-03-12 (annotated)

Committer:
zeutel
Date:
Tue Mar 12 13:43:18 2019 +0000
Revision:
5:03d31f0a4943
Parent:
3:1f93c4510fb1
Child:
7:1f2508ade0a3
I AM EQ!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zeutel 0:cebf1f73ffd5 1 #include "mbed.h"
zeutel 0:cebf1f73ffd5 2 #include "LPS331_I2C.h"
zeutel 0:cebf1f73ffd5 3 #include "LSM9DS1.h"
zeutel 0:cebf1f73ffd5 4 #include "SDFileSystem.h"
zeutel 0:cebf1f73ffd5 5 #include "IM920.h"
Nozaryo 3:1f93c4510fb1 6 #include "millis.h"
zeutel 0:cebf1f73ffd5 7 #include "GPS.h"
zeutel 0:cebf1f73ffd5 8 #include "LPS22HB.h"
Nozaryo 3:1f93c4510fb1 9 #include "stdio.h"
zeutel 0:cebf1f73ffd5 10
zeutel 0:cebf1f73ffd5 11 //ピン
Nozaryo 2:cad76b5be4ba 12 DigitalIn flightpin(p12);
zeutel 0:cebf1f73ffd5 13 LPS331_I2C lps331(p9, p10, LPS331_I2C_SA0_HIGH);
zeutel 0:cebf1f73ffd5 14 LSM9DS1 lsm(p9,p10);
zeutel 0:cebf1f73ffd5 15 SDFileSystem sd(p5, p6, p7, p8, "sd");
zeutel 0:cebf1f73ffd5 16 IM920 im920(p28, p27, p29, p30);
zeutel 0:cebf1f73ffd5 17 GPS gps(p13, p14);
zeutel 0:cebf1f73ffd5 18 LPS22HB lps33hw(p9,p10,LPS22HB_G_CHIP_ADDR);
Nozaryo 3:1f93c4510fb1 19 Timer waitTime;
Nozaryo 3:1f93c4510fb1 20 char sendData[256];
zeutel 0:cebf1f73ffd5 21 PwmOut servo_1(p22), servo_2(p25);
Nozaryo 3:1f93c4510fb1 22 bool SDisAvailable=false;
zeutel 5:03d31f0a4943 23
zeutel 0:cebf1f73ffd5 24 //関数
zeutel 0:cebf1f73ffd5 25 void init(FILE **file);//諸々の初期化
zeutel 0:cebf1f73ffd5 26 void checkingSensors();
zeutel 0:cebf1f73ffd5 27 void readValues(int sequence);//センサの読み取り
zeutel 0:cebf1f73ffd5 28 float calcAltitude(float pres, float temp);//高度計算
zeutel 0:cebf1f73ffd5 29
zeutel 0:cebf1f73ffd5 30 int main() {
zeutel 0:cebf1f73ffd5 31 int sequence=0;//機体の状態の推移
zeutel 5:03d31f0a4943 32 int pretime=0,pretime_2=0;
zeutel 0:cebf1f73ffd5 33 float altitude,maxAltitude=0.0;//m単位
zeutel 0:cebf1f73ffd5 34 float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持
zeutel 0:cebf1f73ffd5 35 char receive[9]={};//受信した文字列
zeutel 0:cebf1f73ffd5 36 bool flightpinAttached=false;//フライトピンが付いているかどうか
Nozaryo 3:1f93c4510fb1 37 millisStart();
Nozaryo 3:1f93c4510fb1 38
zeutel 0:cebf1f73ffd5 39 FILE *fp;
Nozaryo 3:1f93c4510fb1 40 //サーボclose
zeutel 5:03d31f0a4943 41 servo_1.pulsewidth(1.05/1000.0);//ms/1000
Nozaryo 3:1f93c4510fb1 42 servo_2.pulsewidth(1.94/1000.0);
zeutel 0:cebf1f73ffd5 43
zeutel 0:cebf1f73ffd5 44 init(&fp);//初期化
Nozaryo 3:1f93c4510fb1 45 flightpinAttached=flightpin;
zeutel 0:cebf1f73ffd5 46
Nozaryo 3:1f93c4510fb1 47 while(flightpinAttached == false){
Nozaryo 3:1f93c4510fb1 48 im920.poll();
Nozaryo 3:1f93c4510fb1 49 memset(receive,'\0',sizeof(receive));
Nozaryo 3:1f93c4510fb1 50 im920.recv(receive,8);
Nozaryo 3:1f93c4510fb1 51 flightpinAttached=flightpin;
zeutel 5:03d31f0a4943 52 if(millis()-pretime>5000){
zeutel 5:03d31f0a4943 53 im920.send("Waiting_1",10);
zeutel 5:03d31f0a4943 54 pretime=millis();
zeutel 5:03d31f0a4943 55 }
zeutel 5:03d31f0a4943 56 if(strncmp(receive,"nowait",sizeof(receive))==0){
zeutel 5:03d31f0a4943 57 flightpinAttached=false;
zeutel 5:03d31f0a4943 58 break;
zeutel 5:03d31f0a4943 59 }
zeutel 5:03d31f0a4943 60 if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
zeutel 5:03d31f0a4943 61 im920.send("Retry initializing",19);
zeutel 5:03d31f0a4943 62 init(&fp);
zeutel 5:03d31f0a4943 63 wait(1);
zeutel 5:03d31f0a4943 64 }
zeutel 5:03d31f0a4943 65 if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
zeutel 5:03d31f0a4943 66 im920.send("Mbed reset",11);
zeutel 5:03d31f0a4943 67 NVIC_SystemReset();
zeutel 5:03d31f0a4943 68 }
zeutel 5:03d31f0a4943 69 }
zeutel 5:03d31f0a4943 70 while(flightpinAttached == true){//組み立て中はつけたまま
zeutel 5:03d31f0a4943 71 im920.poll();
zeutel 5:03d31f0a4943 72 memset(receive,'\0',sizeof(receive));
zeutel 5:03d31f0a4943 73 im920.recv(receive,8);
zeutel 5:03d31f0a4943 74 flightpinAttached=flightpin;
zeutel 5:03d31f0a4943 75 if(millis()-pretime>5000){
zeutel 5:03d31f0a4943 76 im920.send("Waiting_2",10);
zeutel 5:03d31f0a4943 77 pretime=millis();
zeutel 5:03d31f0a4943 78 }
Nozaryo 3:1f93c4510fb1 79 if(strncmp(receive,"nowait",sizeof(receive))==0){
Nozaryo 3:1f93c4510fb1 80 flightpinAttached=false;
Nozaryo 3:1f93c4510fb1 81 break;
Nozaryo 3:1f93c4510fb1 82 }
zeutel 5:03d31f0a4943 83 if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
zeutel 5:03d31f0a4943 84 im920.send("Retry initializing",19);
zeutel 5:03d31f0a4943 85 init(&fp);
zeutel 5:03d31f0a4943 86 wait(1);
zeutel 5:03d31f0a4943 87 }
zeutel 5:03d31f0a4943 88 if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
zeutel 5:03d31f0a4943 89 im920.send("Mbed reset",11);
zeutel 5:03d31f0a4943 90 NVIC_SystemReset();
zeutel 5:03d31f0a4943 91 }
Nozaryo 3:1f93c4510fb1 92 }
zeutel 5:03d31f0a4943 93 while(flightpinAttached == false){//ランチャー取り付け中は取り外す
zeutel 5:03d31f0a4943 94 im920.poll();
zeutel 5:03d31f0a4943 95 memset(receive,'\0',sizeof(receive));
zeutel 5:03d31f0a4943 96 im920.recv(receive,8);
Nozaryo 3:1f93c4510fb1 97 flightpinAttached=flightpin;
zeutel 5:03d31f0a4943 98 if(millis()-pretime>5000){
zeutel 5:03d31f0a4943 99 im920.send("Waiting_3",10);
zeutel 5:03d31f0a4943 100 pretime=millis();
zeutel 5:03d31f0a4943 101 }
zeutel 5:03d31f0a4943 102 if(strncmp(receive,"nowait",sizeof(receive))==0){
zeutel 5:03d31f0a4943 103 flightpinAttached=false;
zeutel 5:03d31f0a4943 104 break;
zeutel 5:03d31f0a4943 105 }
zeutel 5:03d31f0a4943 106 if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
zeutel 5:03d31f0a4943 107 im920.send("Retry initializing",19);
zeutel 5:03d31f0a4943 108 init(&fp);
zeutel 5:03d31f0a4943 109 wait(1);
zeutel 5:03d31f0a4943 110 }
zeutel 5:03d31f0a4943 111 if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
zeutel 5:03d31f0a4943 112 im920.send("Mbed reset",11);
zeutel 5:03d31f0a4943 113 NVIC_SystemReset();
zeutel 5:03d31f0a4943 114 }
Nozaryo 3:1f93c4510fb1 115 }
zeutel 5:03d31f0a4943 116 im920.send("escape Waiting",14);
zeutel 0:cebf1f73ffd5 117
zeutel 5:03d31f0a4943 118 while(strncmp(receive,"start",sizeof(receive))!=0){//無線での開始指令待ち txda startと受信したら計測開始
zeutel 0:cebf1f73ffd5 119 im920.poll();
zeutel 0:cebf1f73ffd5 120 memset(receive,'\0',sizeof(receive));
zeutel 0:cebf1f73ffd5 121 im920.recv(receive,8);
zeutel 0:cebf1f73ffd5 122 //加速度・フライトピン読み取り
zeutel 0:cebf1f73ffd5 123 lsm.readAccel();
zeutel 0:cebf1f73ffd5 124 flightpinAttached=flightpin;
zeutel 0:cebf1f73ffd5 125 if(strncmp(receive,"check",sizeof(receive))==0){//checkと受信したらセンサーの状態確認 主にGPS
zeutel 0:cebf1f73ffd5 126 im920.send("Check",6);
zeutel 0:cebf1f73ffd5 127 checkingSensors();
zeutel 0:cebf1f73ffd5 128 }
zeutel 0:cebf1f73ffd5 129 if(strncmp(receive,"retry",sizeof(receive))==0){//retryと受信したら再度初期化
zeutel 0:cebf1f73ffd5 130 im920.send("Retry initializing",19);
zeutel 0:cebf1f73ffd5 131 init(&fp);
zeutel 5:03d31f0a4943 132 wait(1);
zeutel 0:cebf1f73ffd5 133 }
zeutel 0:cebf1f73ffd5 134 if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
zeutel 0:cebf1f73ffd5 135 im920.send("Mbed reset",11);
zeutel 0:cebf1f73ffd5 136 NVIC_SystemReset();
zeutel 0:cebf1f73ffd5 137 }
zeutel 5:03d31f0a4943 138 if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=4.0 && flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始
zeutel 0:cebf1f73ffd5 139 im920.send("Forced start",13);
zeutel 5:03d31f0a4943 140 launchTime=millis();
zeutel 5:03d31f0a4943 141 sequence=1;
zeutel 0:cebf1f73ffd5 142 break;
zeutel 0:cebf1f73ffd5 143 }
Nozaryo 3:1f93c4510fb1 144 //地上局への送信
Nozaryo 3:1f93c4510fb1 145 if(millis() - pretime > 5000){//5秒おきに経過時間送信
zeutel 5:03d31f0a4943 146 lps33hw.get();
zeutel 5:03d31f0a4943 147 sprintf(sendData," %06.3fs, %06.2fPa",(float)millis()/1000,lps33hw.pressure());
zeutel 5:03d31f0a4943 148 im920.send(sendData,20);
Nozaryo 3:1f93c4510fb1 149 pretime = millis();
Nozaryo 3:1f93c4510fb1 150 }
zeutel 0:cebf1f73ffd5 151 }
zeutel 0:cebf1f73ffd5 152 im920.send("START",6);
zeutel 0:cebf1f73ffd5 153
zeutel 0:cebf1f73ffd5 154 while(strncmp(receive,"end",sizeof(receive))!=0&&sequence!=3) {//endと受信するまたは着地したら終了
zeutel 0:cebf1f73ffd5 155 //受信内容の初期化
zeutel 0:cebf1f73ffd5 156 im920.poll();
zeutel 0:cebf1f73ffd5 157 memset(receive,'\0',sizeof(receive));
zeutel 0:cebf1f73ffd5 158 im920.recv(receive,8);
zeutel 0:cebf1f73ffd5 159
zeutel 0:cebf1f73ffd5 160 if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
zeutel 0:cebf1f73ffd5 161 im920.send("Mbed reset",11);
zeutel 0:cebf1f73ffd5 162 NVIC_SystemReset();
zeutel 0:cebf1f73ffd5 163 }
zeutel 0:cebf1f73ffd5 164
zeutel 0:cebf1f73ffd5 165 //センサ・フライトピン読み取り 高度計測
zeutel 0:cebf1f73ffd5 166 readValues(sequence);
zeutel 0:cebf1f73ffd5 167 flightpinAttached=flightpin;
zeutel 5:03d31f0a4943 168 altitude=calcAltitude(lps33hw.pressure(),lps33hw.temperature());
zeutel 0:cebf1f73ffd5 169
zeutel 0:cebf1f73ffd5 170 if(maxAltitude<altitude){//最高高度更新
zeutel 0:cebf1f73ffd5 171 maxAltitude=altitude;
zeutel 0:cebf1f73ffd5 172 }
zeutel 0:cebf1f73ffd5 173
zeutel 0:cebf1f73ffd5 174 switch(sequence){
zeutel 0:cebf1f73ffd5 175 case 0://発射待機中
zeutel 0:cebf1f73ffd5 176 //離床検知
zeutel 5:03d31f0a4943 177 if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=4.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている
zeutel 0:cebf1f73ffd5 178 im920.send("LAUNCH ACC>2.0G",16);
Nozaryo 3:1f93c4510fb1 179 launchTime=millis();
Nozaryo 3:1f93c4510fb1 180 sequence=1;
Nozaryo 3:1f93c4510fb1 181 }
zeutel 5:03d31f0a4943 182 if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=3.9*3.9 &&flightpinAttached==true){//加速度4.0G以上かつフライトピンがぬけてない
zeutel 5:03d31f0a4943 183 im920.send("LAUNCH ACC>3.9G",16);
Nozaryo 3:1f93c4510fb1 184 launchTime=millis();
zeutel 0:cebf1f73ffd5 185 sequence=1;
zeutel 0:cebf1f73ffd5 186 }
zeutel 0:cebf1f73ffd5 187 break;
zeutel 0:cebf1f73ffd5 188
zeutel 0:cebf1f73ffd5 189 case 1://発射～減速機構展開
zeutel 0:cebf1f73ffd5 190 //減速機構展開
zeutel 5:03d31f0a4943 191 if(millis()-launchTime>=10*1000){
zeutel 5:03d31f0a4943 192 if(millis()-launchTime>=15*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下
zeutel 5:03d31f0a4943 193 im920.send("Start speed reducing",21);
zeutel 5:03d31f0a4943 194 reducerExpantionedTime=millis();
zeutel 5:03d31f0a4943 195 //サーボopen
zeutel 5:03d31f0a4943 196 servo_1.pulsewidth(1.90/1000);//ms/1000
zeutel 5:03d31f0a4943 197 servo_2.pulsewidth(1.32/1000);
zeutel 5:03d31f0a4943 198 sequence=2;
zeutel 5:03d31f0a4943 199 }
zeutel 0:cebf1f73ffd5 200 }
zeutel 0:cebf1f73ffd5 201 break;
zeutel 0:cebf1f73ffd5 202
zeutel 0:cebf1f73ffd5 203 case 2://減速機構展開～着地
Nozaryo 3:1f93c4510fb1 204 if(millis()-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過
zeutel 0:cebf1f73ffd5 205 im920.send("LANDING",8);
zeutel 0:cebf1f73ffd5 206 sequence=3;
zeutel 0:cebf1f73ffd5 207 }
zeutel 0:cebf1f73ffd5 208 break;
zeutel 0:cebf1f73ffd5 209 }
zeutel 0:cebf1f73ffd5 210
zeutel 0:cebf1f73ffd5 211 //SD書き込み time,temp,pres,temp_hw,pres_hw,accXYZ,gyroXYZ,magnXYZ,longitude,latitude
Nozaryo 3:1f93c4510fb1 212 if(SDisAvailable){
Nozaryo 3:1f93c4510fb1 213 if(sequence==1){//飛行中
zeutel 5:03d31f0a4943 214 fprintf(fp,"%.3f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
zeutel 5:03d31f0a4943 215 (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);
Nozaryo 3:1f93c4510fb1 216 }else{//飛行中以外はGPS情報も保存
zeutel 5:03d31f0a4943 217 fprintf(fp,"%.3f,%f,%f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n",
zeutel 5:03d31f0a4943 218 (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);
Nozaryo 3:1f93c4510fb1 219 }
zeutel 0:cebf1f73ffd5 220 }
zeutel 5:03d31f0a4943 221 if(millis() - pretime_2 > 2000){//2秒おきにGPS送信
zeutel 5:03d31f0a4943 222 sprintf(sendData," %1d: %06.2fm, %07.3fs",sequence,altitude,(float)millis()/1000);
zeutel 5:03d31f0a4943 223 im920.send(sendData,22);
zeutel 5:03d31f0a4943 224 pretime_2=millis();
zeutel 5:03d31f0a4943 225 }
zeutel 0:cebf1f73ffd5 226
Nozaryo 3:1f93c4510fb1 227 //地上局へのGPS情報送信
zeutel 5:03d31f0a4943 228 if(millis() - pretime > 5000){//5秒おきにGPS送信
zeutel 5:03d31f0a4943 229 if(sequence != 1){
zeutel 5:03d31f0a4943 230 waitTime.start();
zeutel 5:03d31f0a4943 231 waitTime.reset();
zeutel 5:03d31f0a4943 232 while(gps.result==false){//GPSが利用可能になるまでまつ
zeutel 5:03d31f0a4943 233 gps.getgps();
zeutel 5:03d31f0a4943 234 if(waitTime.read() > 1){
zeutel 5:03d31f0a4943 235 waitTime.stop();
zeutel 5:03d31f0a4943 236 break;
zeutel 5:03d31f0a4943 237 }
Nozaryo 3:1f93c4510fb1 238 }
zeutel 5:03d31f0a4943 239 sprintf(sendData," %08.5fE, %08.5fN",gps.longitude,gps.latitude);
zeutel 5:03d31f0a4943 240 im920.send(sendData,24);
Nozaryo 3:1f93c4510fb1 241 }
zeutel 5:03d31f0a4943 242 pretime=millis();
zeutel 5:03d31f0a4943 243 }
zeutel 5:03d31f0a4943 244 }
zeutel 5:03d31f0a4943 245 if(SDisAvailable){
zeutel 5:03d31f0a4943 246 fclose(fp);
zeutel 5:03d31f0a4943 247 im920.send("File closed",12);
zeutel 5:03d31f0a4943 248 }else{
zeutel 5:03d31f0a4943 249 im920.send("SD error",9);
zeutel 5:03d31f0a4943 250 }
zeutel 5:03d31f0a4943 251 sprintf(sendData," MaxAltitude: %06.2fm",maxAltitude);
zeutel 5:03d31f0a4943 252 im920.send(sendData,23);
zeutel 5:03d31f0a4943 253 while(1){
zeutel 5:03d31f0a4943 254 //受信内容の初期化
zeutel 5:03d31f0a4943 255 im920.poll();
zeutel 5:03d31f0a4943 256 memset(receive,'\0',sizeof(receive));
zeutel 5:03d31f0a4943 257 im920.recv(receive,8);
zeutel 5:03d31f0a4943 258
zeutel 5:03d31f0a4943 259 if(strncmp(receive,"reset",sizeof(receive))==0){//resetと受信したらMbedリセット
zeutel 5:03d31f0a4943 260 im920.send("Mbed reset",11);
zeutel 5:03d31f0a4943 261 NVIC_SystemReset();
zeutel 5:03d31f0a4943 262 }
zeutel 5:03d31f0a4943 263
zeutel 5:03d31f0a4943 264 if(millis() - pretime > 5000){//5秒おきにGPS送信
zeutel 5:03d31f0a4943 265 if(sequence != 1){
zeutel 5:03d31f0a4943 266 waitTime.start();
zeutel 5:03d31f0a4943 267 waitTime.reset();
zeutel 5:03d31f0a4943 268 while(gps.result==false){//GPSが利用可能になるまでまつ
zeutel 5:03d31f0a4943 269 gps.getgps();
zeutel 5:03d31f0a4943 270 if(waitTime.read() > 1){
zeutel 5:03d31f0a4943 271 waitTime.stop();
zeutel 5:03d31f0a4943 272 break;
zeutel 5:03d31f0a4943 273 }
zeutel 5:03d31f0a4943 274 }
zeutel 5:03d31f0a4943 275 sprintf(sendData," %08.5fE, %08.5fN",gps.longitude,gps.latitude);
zeutel 5:03d31f0a4943 276 im920.send(sendData,24);
zeutel 5:03d31f0a4943 277 }
zeutel 5:03d31f0a4943 278 pretime=millis();
Nozaryo 3:1f93c4510fb1 279 }
zeutel 0:cebf1f73ffd5 280 }
zeutel 0:cebf1f73ffd5 281 }
zeutel 0:cebf1f73ffd5 282
zeutel 0:cebf1f73ffd5 283 void init(FILE **file){
zeutel 0:cebf1f73ffd5 284 //im920
zeutel 0:cebf1f73ffd5 285 im920.init();
zeutel 0:cebf1f73ffd5 286 wait(1);
zeutel 0:cebf1f73ffd5 287
zeutel 0:cebf1f73ffd5 288 //MMTXS03
zeutel 0:cebf1f73ffd5 289 if(lps331.isAvailable()) {
zeutel 0:cebf1f73ffd5 290 im920.send("MM-TXS03...OK",14);
zeutel 0:cebf1f73ffd5 291 } else {
zeutel 0:cebf1f73ffd5 292 im920.send("MM-TXS03...NG",14);
zeutel 0:cebf1f73ffd5 293 }
zeutel 0:cebf1f73ffd5 294
zeutel 0:cebf1f73ffd5 295 //MM9DS1
zeutel 0:cebf1f73ffd5 296 lsm.begin();
zeutel 0:cebf1f73ffd5 297 if(lsm.whoAmI()){
zeutel 0:cebf1f73ffd5 298 im920.send("MM-9DS1...OK",13);
zeutel 0:cebf1f73ffd5 299 }else {
zeutel 0:cebf1f73ffd5 300 im920.send("MM-9DS1...NG",13);
zeutel 0:cebf1f73ffd5 301 }
zeutel 0:cebf1f73ffd5 302
zeutel 0:cebf1f73ffd5 303 //LPS33HW
zeutel 5:03d31f0a4943 304 if(!lps33hw.whoAmI()){
zeutel 0:cebf1f73ffd5 305 im920.send("LPS33HW...OK",13);
zeutel 0:cebf1f73ffd5 306 }else {
zeutel 0:cebf1f73ffd5 307 im920.send("LPS33HW...NG",13);
zeutel 0:cebf1f73ffd5 308 }
Nozaryo 3:1f93c4510fb1 309 wait(1);
zeutel 5:03d31f0a4943 310
zeutel 0:cebf1f73ffd5 311 //SD
zeutel 0:cebf1f73ffd5 312 *file = fopen("/sd/data.csv", "w");
zeutel 0:cebf1f73ffd5 313 if(*file == NULL) {
zeutel 0:cebf1f73ffd5 314 im920.send("SD...NG",8);
Nozaryo 3:1f93c4510fb1 315 SDisAvailable = false;
zeutel 0:cebf1f73ffd5 316 }else{
zeutel 0:cebf1f73ffd5 317 im920.send("SD...OK",8);
zeutel 5:03d31f0a4943 318 fprintf(*file,"time,temperature,pressure,temperature_hw,pressure_hw,accX,accY,accZ,gyroX,gyroY,gyroZ,magnX,magnY,magnZ,longitude,latitude\r\n");
Nozaryo 3:1f93c4510fb1 319 SDisAvailable = true;
zeutel 0:cebf1f73ffd5 320 }
zeutel 0:cebf1f73ffd5 321
zeutel 0:cebf1f73ffd5 322 //GPS優先度
zeutel 0:cebf1f73ffd5 323 NVIC_SetPriority(UART3_IRQn,0);
zeutel 0:cebf1f73ffd5 324 gps.getgps();
zeutel 0:cebf1f73ffd5 325 if(gps.result){
zeutel 0:cebf1f73ffd5 326 im920.send("GPS...OK",9);
zeutel 5:03d31f0a4943 327 sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
zeutel 5:03d31f0a4943 328 im920.send(sendData,11);
zeutel 0:cebf1f73ffd5 329 }else{
zeutel 0:cebf1f73ffd5 330 im920.send("GPS...NG",9);
zeutel 0:cebf1f73ffd5 331 }
zeutel 0:cebf1f73ffd5 332
zeutel 0:cebf1f73ffd5 333 //フライトピン
Nozaryo 2:cad76b5be4ba 334 //flightpin.output();
zeutel 0:cebf1f73ffd5 335
zeutel 0:cebf1f73ffd5 336 //各種設定
zeutel 5:03d31f0a4943 337 /*lps331.setResolution(LPS331_I2C_PRESSURE_AVG_512, LPS331_I2C_TEMP_AVG_128);
zeutel 0:cebf1f73ffd5 338 lps331.setDataRate(LPS331_I2C_DATARATE_25HZ_T);
zeutel 0:cebf1f73ffd5 339 lps331.setActive(true);
zeutel 0:cebf1f73ffd5 340 lsm.setAccelODR(lsm.A_ODR_952);
zeutel 0:cebf1f73ffd5 341 lsm.setGyroODR(lsm.G_ODR_952_BW_100);
zeutel 5:03d31f0a4943 342 lsm.setMagODR(lsm.M_ODR_80);*/
zeutel 0:cebf1f73ffd5 343 }
zeutel 0:cebf1f73ffd5 344
zeutel 0:cebf1f73ffd5 345 void checkingSensors(){
zeutel 0:cebf1f73ffd5 346 //MMTXS03
zeutel 0:cebf1f73ffd5 347 if(lps331.isAvailable()) {
zeutel 0:cebf1f73ffd5 348 im920.send("MM-TXS03...OK",14);
zeutel 0:cebf1f73ffd5 349 } else {
zeutel 0:cebf1f73ffd5 350 im920.send("MM-TXS03...NG",14);
zeutel 0:cebf1f73ffd5 351 }
zeutel 0:cebf1f73ffd5 352
zeutel 0:cebf1f73ffd5 353 //MM9DS1
zeutel 0:cebf1f73ffd5 354 if(lsm.whoAmI()){
zeutel 0:cebf1f73ffd5 355 im920.send("MM-9DS1...OK",13);
zeutel 0:cebf1f73ffd5 356 }else {
zeutel 0:cebf1f73ffd5 357 im920.send("MM-9DS1...NG",13);
zeutel 0:cebf1f73ffd5 358 }
zeutel 0:cebf1f73ffd5 359
zeutel 0:cebf1f73ffd5 360 //LPS33HW
Nozaryo 2:cad76b5be4ba 361 if(!lps33hw.whoAmI()){
zeutel 0:cebf1f73ffd5 362 im920.send("LPS33HW...OK",13);
zeutel 0:cebf1f73ffd5 363 }else {
zeutel 0:cebf1f73ffd5 364 im920.send("LPS33HW...NG",13);
zeutel 0:cebf1f73ffd5 365 }
zeutel 0:cebf1f73ffd5 366
zeutel 0:cebf1f73ffd5 367 //GPS
zeutel 0:cebf1f73ffd5 368 gps.getgps();
zeutel 0:cebf1f73ffd5 369 if(gps.result){
zeutel 0:cebf1f73ffd5 370 im920.send("GPS...OK",9);
zeutel 5:03d31f0a4943 371 sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude);
zeutel 5:03d31f0a4943 372 im920.send(sendData,11);
zeutel 0:cebf1f73ffd5 373 }else{
zeutel 0:cebf1f73ffd5 374 im920.send("GPS...NG",9);
zeutel 0:cebf1f73ffd5 375 }
zeutel 0:cebf1f73ffd5 376 }
zeutel 0:cebf1f73ffd5 377
zeutel 0:cebf1f73ffd5 378 void readValues(int sequence){
zeutel 0:cebf1f73ffd5 379 lsm.readAccel();
zeutel 0:cebf1f73ffd5 380 lsm.readGyro();
zeutel 0:cebf1f73ffd5 381 lsm.readMag();
zeutel 0:cebf1f73ffd5 382 lps331.getPressure();
zeutel 0:cebf1f73ffd5 383 lps331.getTemperature();
zeutel 0:cebf1f73ffd5 384 lps33hw.get();
zeutel 0:cebf1f73ffd5 385 if(sequence!=1){
zeutel 0:cebf1f73ffd5 386 gps.getgps();
zeutel 0:cebf1f73ffd5 387 }
zeutel 0:cebf1f73ffd5 388 }
zeutel 0:cebf1f73ffd5 389
zeutel 0:cebf1f73ffd5 390 float calcAltitude(float pres, float temp){
zeutel 0:cebf1f73ffd5 391 return (pow(1013.25/pres,1/5.257)-1)*(temp+273.15)/0.0065;
zeutel 0:cebf1f73ffd5 392 }