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