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?
User | Revision | Line number | New 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 | } |