![](/media/cache/group/TohokuFTE_400x400.jpg.50x50_q85.jpg)
2019伊豆のコード
Dependencies: mbed LSM9DS1 IM920 SDFileSystem LPS331 ADXL345_I2C GPS_Interface millis
main.cpp@8:b358cd848797, 2020-09-25 (annotated)
- Committer:
- zeutel
- Date:
- Fri Sep 25 14:33:10 2020 +0000
- Revision:
- 8:b358cd848797
- Parent:
- 7:1f2508ade0a3
commit
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 | 7:1f2508ade0a3 | 7 | #include "ADXL345_I2C.h" |
zeutel | 0:cebf1f73ffd5 | 8 | #include "GPS.h" |
zeutel | 7:1f2508ade0a3 | 9 | #include <stdio.h> |
zeutel | 7:1f2508ade0a3 | 10 | #include <string.h> |
zeutel | 0:cebf1f73ffd5 | 11 | |
zeutel | 7:1f2508ade0a3 | 12 | //ピンとか |
zeutel | 7:1f2508ade0a3 | 13 | Serial pc(USBTX, USBRX); |
zeutel | 7:1f2508ade0a3 | 14 | DigitalIn flightpin(p12);//フライトピン |
zeutel | 7:1f2508ade0a3 | 15 | LPS331_I2C lps331(p9, p10, LPS331_I2C_SA0_HIGH);//気温気圧 |
zeutel | 7:1f2508ade0a3 | 16 | ADXL345_I2C accelerometer(p9, p10);//オープニングショック |
zeutel | 7:1f2508ade0a3 | 17 | LSM9DS1 lsm(p9,p10);//9軸 |
zeutel | 7:1f2508ade0a3 | 18 | IM920 im920(p28, p27, p29, p30);//無線 |
zeutel | 7:1f2508ade0a3 | 19 | GPS gps(p13, p14);//GPS |
zeutel | 7:1f2508ade0a3 | 20 | SDFileSystem sd(p5, p6, p7, p8, "sd");//SD |
zeutel | 7:1f2508ade0a3 | 21 | PwmOut servo_1(p26), servo_2(p25);//サーボ |
Nozaryo | 3:1f93c4510fb1 | 22 | Timer waitTime; |
zeutel | 7:1f2508ade0a3 | 23 | |
zeutel | 5:03d31f0a4943 | 24 | |
zeutel | 7:1f2508ade0a3 | 25 | |
zeutel | 7:1f2508ade0a3 | 26 | //function |
zeutel | 0:cebf1f73ffd5 | 27 | void init(FILE **file);//諸々の初期化 |
zeutel | 0:cebf1f73ffd5 | 28 | void checkingSensors(); |
zeutel | 7:1f2508ade0a3 | 29 | void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings);//センサの読み取り |
zeutel | 7:1f2508ade0a3 | 30 | void imRecv(char *receive);//無線受信 |
zeutel | 0:cebf1f73ffd5 | 31 | float calcAltitude(float pres, float temp);//高度計算 |
zeutel | 7:1f2508ade0a3 | 32 | void imSend(char *send); |
zeutel | 7:1f2508ade0a3 | 33 | void mbedOperate(char *command); |
zeutel | 7:1f2508ade0a3 | 34 | void sendDatas(float time,float temp,float pres,float ax,float ay,float az,float gx,float gy,float gz);//unity用data送信 |
zeutel | 7:1f2508ade0a3 | 35 | |
zeutel | 7:1f2508ade0a3 | 36 | //global |
zeutel | 7:1f2508ade0a3 | 37 | char sendData[256]; |
zeutel | 7:1f2508ade0a3 | 38 | FILE *fp; |
zeutel | 7:1f2508ade0a3 | 39 | bool SDisAvailable=false; |
zeutel | 0:cebf1f73ffd5 | 40 | |
zeutel | 0:cebf1f73ffd5 | 41 | int main() { |
zeutel | 0:cebf1f73ffd5 | 42 | int sequence=0;//機体の状態の推移 |
zeutel | 5:03d31f0a4943 | 43 | int pretime=0,pretime_2=0; |
zeutel | 7:1f2508ade0a3 | 44 | float altitude=-1000.0,maxAltitude=-1000.0;//m単位 |
zeutel | 0:cebf1f73ffd5 | 45 | float launchTime,reducerExpantionedTime;//発射・減速機構展開時間の保持 |
zeutel | 0:cebf1f73ffd5 | 46 | char receive[9]={};//受信した文字列 |
zeutel | 0:cebf1f73ffd5 | 47 | bool flightpinAttached=false;//フライトピンが付いているかどうか |
zeutel | 7:1f2508ade0a3 | 48 | float temperature,pressure; |
zeutel | 7:1f2508ade0a3 | 49 | float accelReadings[3] = {0, 0, 0};//x,y,z |
zeutel | 7:1f2508ade0a3 | 50 | |
Nozaryo | 3:1f93c4510fb1 | 51 | millisStart(); |
zeutel | 7:1f2508ade0a3 | 52 | |
Nozaryo | 3:1f93c4510fb1 | 53 | //サーボclose |
zeutel | 5:03d31f0a4943 | 54 | servo_1.pulsewidth(1.05/1000.0);//ms/1000 |
Nozaryo | 3:1f93c4510fb1 | 55 | servo_2.pulsewidth(1.94/1000.0); |
zeutel | 0:cebf1f73ffd5 | 56 | |
zeutel | 0:cebf1f73ffd5 | 57 | init(&fp);//初期化 |
Nozaryo | 3:1f93c4510fb1 | 58 | flightpinAttached=flightpin; |
zeutel | 0:cebf1f73ffd5 | 59 | |
zeutel | 7:1f2508ade0a3 | 60 | while(strncmp(receive,"escape",sizeof(receive))!=0){//準備中 |
zeutel | 7:1f2508ade0a3 | 61 | imRecv(receive); |
zeutel | 7:1f2508ade0a3 | 62 | mbedOperate(receive); |
zeutel | 7:1f2508ade0a3 | 63 | readValues(sequence,&temperature,&pressure,accelReadings); |
zeutel | 7:1f2508ade0a3 | 64 | sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz); |
zeutel | 5:03d31f0a4943 | 65 | if(millis()-pretime>5000){ |
zeutel | 5:03d31f0a4943 | 66 | pretime=millis(); |
zeutel | 7:1f2508ade0a3 | 67 | //センサ・フライトピン |
zeutel | 7:1f2508ade0a3 | 68 | flightpinAttached=flightpin; |
zeutel | 7:1f2508ade0a3 | 69 | //altitude=calcAltitude(pressure,temperature); |
zeutel | 7:1f2508ade0a3 | 70 | //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)); |
zeutel | 7:1f2508ade0a3 | 71 | //imSend(sendData); |
zeutel | 7:1f2508ade0a3 | 72 | imSend("Waiting: command 'escape' to escape waiting"); |
zeutel | 7:1f2508ade0a3 | 73 | if(flightpinAttached==false){ |
zeutel | 7:1f2508ade0a3 | 74 | imSend("flightpin isn't attached"); |
zeutel | 7:1f2508ade0a3 | 75 | } |
zeutel | 5:03d31f0a4943 | 76 | } |
Nozaryo | 3:1f93c4510fb1 | 77 | } |
zeutel | 7:1f2508ade0a3 | 78 | |
zeutel | 7:1f2508ade0a3 | 79 | imSend("Escape Waiting"); |
zeutel | 7:1f2508ade0a3 | 80 | |
zeutel | 7:1f2508ade0a3 | 81 | while(strncmp(receive,"record",sizeof(receive))!=0){//開始指令待ち |
zeutel | 7:1f2508ade0a3 | 82 | imRecv(receive); |
zeutel | 7:1f2508ade0a3 | 83 | mbedOperate(receive); |
zeutel | 7:1f2508ade0a3 | 84 | //加速度・フライトピン読み取り |
zeutel | 7:1f2508ade0a3 | 85 | readValues(sequence,&temperature,&pressure,accelReadings); |
zeutel | 7:1f2508ade0a3 | 86 | sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz); |
Nozaryo | 3:1f93c4510fb1 | 87 | flightpinAttached=flightpin; |
zeutel | 7:1f2508ade0a3 | 88 | if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0*2.0 && flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけていたらコマンドがなくても開始 |
zeutel | 7:1f2508ade0a3 | 89 | imSend("Forced start"); |
zeutel | 5:03d31f0a4943 | 90 | launchTime=millis(); |
zeutel | 5:03d31f0a4943 | 91 | sequence=1; |
zeutel | 0:cebf1f73ffd5 | 92 | break; |
zeutel | 0:cebf1f73ffd5 | 93 | } |
Nozaryo | 3:1f93c4510fb1 | 94 | //地上局への送信 |
Nozaryo | 3:1f93c4510fb1 | 95 | if(millis() - pretime > 5000){//5秒おきに経過時間送信 |
zeutel | 7:1f2508ade0a3 | 96 | //altitude=calcAltitude(pressure,temperature); |
zeutel | 7:1f2508ade0a3 | 97 | //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)); |
zeutel | 7:1f2508ade0a3 | 98 | //imSend(sendData); |
zeutel | 7:1f2508ade0a3 | 99 | imSend("Start Waiting: command 'record' to start recording"); |
Nozaryo | 3:1f93c4510fb1 | 100 | pretime = millis(); |
Nozaryo | 3:1f93c4510fb1 | 101 | } |
zeutel | 0:cebf1f73ffd5 | 102 | } |
zeutel | 7:1f2508ade0a3 | 103 | imSend("RECORDEING START"); |
zeutel | 0:cebf1f73ffd5 | 104 | |
zeutel | 0:cebf1f73ffd5 | 105 | while(strncmp(receive,"end",sizeof(receive))!=0&&sequence!=3) {//endと受信するまたは着地したら終了 |
zeutel | 7:1f2508ade0a3 | 106 | imRecv(receive); |
zeutel | 7:1f2508ade0a3 | 107 | mbedOperate(receive); |
zeutel | 0:cebf1f73ffd5 | 108 | //センサ・フライトピン読み取り 高度計測 |
zeutel | 7:1f2508ade0a3 | 109 | readValues(sequence,&temperature,&pressure,accelReadings); |
zeutel | 7:1f2508ade0a3 | 110 | sendDatas((float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz); |
zeutel | 0:cebf1f73ffd5 | 111 | flightpinAttached=flightpin; |
zeutel | 7:1f2508ade0a3 | 112 | altitude=calcAltitude(pressure,temperature); |
zeutel | 0:cebf1f73ffd5 | 113 | |
zeutel | 0:cebf1f73ffd5 | 114 | if(maxAltitude<altitude){//最高高度更新 |
zeutel | 0:cebf1f73ffd5 | 115 | maxAltitude=altitude; |
zeutel | 0:cebf1f73ffd5 | 116 | } |
zeutel | 0:cebf1f73ffd5 | 117 | |
zeutel | 0:cebf1f73ffd5 | 118 | switch(sequence){ |
zeutel | 0:cebf1f73ffd5 | 119 | case 0://発射待機中 |
zeutel | 0:cebf1f73ffd5 | 120 | //離床検知 |
zeutel | 7:1f2508ade0a3 | 121 | if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=2.0*2.0 &&flightpinAttached==false){//加速度2.0G以上かつフライトピンがぬけている |
zeutel | 7:1f2508ade0a3 | 122 | imSend("LAUNCH ACC>2.0G"); |
Nozaryo | 3:1f93c4510fb1 | 123 | launchTime=millis(); |
Nozaryo | 3:1f93c4510fb1 | 124 | sequence=1; |
Nozaryo | 3:1f93c4510fb1 | 125 | } |
zeutel | 5:03d31f0a4943 | 126 | if(lsm.ax*lsm.ax+lsm.ay*lsm.ay+lsm.az*lsm.az>=3.9*3.9 &&flightpinAttached==true){//加速度4.0G以上かつフライトピンがぬけてない |
zeutel | 7:1f2508ade0a3 | 127 | imSend("LAUNCH ACC>3.9G"); |
Nozaryo | 3:1f93c4510fb1 | 128 | launchTime=millis(); |
zeutel | 0:cebf1f73ffd5 | 129 | sequence=1; |
zeutel | 0:cebf1f73ffd5 | 130 | } |
zeutel | 0:cebf1f73ffd5 | 131 | break; |
zeutel | 0:cebf1f73ffd5 | 132 | |
zeutel | 0:cebf1f73ffd5 | 133 | case 1://発射~減速機構展開 |
zeutel | 0:cebf1f73ffd5 | 134 | //減速機構展開 |
zeutel | 7:1f2508ade0a3 | 135 | if(millis()-launchTime>=13*1000){ |
zeutel | 7:1f2508ade0a3 | 136 | if(millis()-launchTime>=15*1000||maxAltitude-altitude>=10){//離床検知から18秒経過 or 最高高度から10m落下 |
zeutel | 7:1f2508ade0a3 | 137 | imSend("Start speed reducing"); |
zeutel | 5:03d31f0a4943 | 138 | reducerExpantionedTime=millis(); |
zeutel | 5:03d31f0a4943 | 139 | //サーボopen |
zeutel | 5:03d31f0a4943 | 140 | servo_1.pulsewidth(1.90/1000);//ms/1000 |
zeutel | 5:03d31f0a4943 | 141 | servo_2.pulsewidth(1.32/1000); |
zeutel | 5:03d31f0a4943 | 142 | sequence=2; |
zeutel | 5:03d31f0a4943 | 143 | } |
zeutel | 0:cebf1f73ffd5 | 144 | } |
zeutel | 0:cebf1f73ffd5 | 145 | break; |
zeutel | 0:cebf1f73ffd5 | 146 | |
zeutel | 0:cebf1f73ffd5 | 147 | case 2://減速機構展開~着地 |
Nozaryo | 3:1f93c4510fb1 | 148 | if(millis()-reducerExpantionedTime>=80*1000){//減速機構展開から80秒経過 |
zeutel | 7:1f2508ade0a3 | 149 | imSend("LANDING"); |
zeutel | 0:cebf1f73ffd5 | 150 | sequence=3; |
zeutel | 0:cebf1f73ffd5 | 151 | } |
zeutel | 0:cebf1f73ffd5 | 152 | break; |
zeutel | 0:cebf1f73ffd5 | 153 | } |
zeutel | 0:cebf1f73ffd5 | 154 | |
zeutel | 7:1f2508ade0a3 | 155 | if(millis() - pretime_2 > 2000){//2秒おきに状態送信 |
zeutel | 5:03d31f0a4943 | 156 | sprintf(sendData," %1d: %06.2fm, %07.3fs",sequence,altitude,(float)millis()/1000); |
zeutel | 7:1f2508ade0a3 | 157 | imSend(sendData); |
zeutel | 5:03d31f0a4943 | 158 | pretime_2=millis(); |
zeutel | 5:03d31f0a4943 | 159 | } |
zeutel | 0:cebf1f73ffd5 | 160 | |
zeutel | 7:1f2508ade0a3 | 161 | if(sequence==1){ |
zeutel | 7:1f2508ade0a3 | 162 | //SD書き込み |
zeutel | 7:1f2508ade0a3 | 163 | if(SDisAvailable){ |
zeutel | 7:1f2508ade0a3 | 164 | fprintf(fp,"%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", |
zeutel | 7:1f2508ade0a3 | 165 | (float)millis()/1000,temperature,pressure,lsm.ax,lsm.ay,lsm.az,lsm.gx,lsm.gy,lsm.gz,lsm.mx,lsm.my,lsm.mz); |
zeutel | 7:1f2508ade0a3 | 166 | } |
zeutel | 7:1f2508ade0a3 | 167 | }else{ |
zeutel | 7:1f2508ade0a3 | 168 | //SD書き込み |
zeutel | 7:1f2508ade0a3 | 169 | if(SDisAvailable){ |
zeutel | 7:1f2508ade0a3 | 170 | fprintf(fp,"%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f\r\n", |
zeutel | 7:1f2508ade0a3 | 171 | (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); |
zeutel | 7:1f2508ade0a3 | 172 | } |
zeutel | 7:1f2508ade0a3 | 173 | |
zeutel | 7:1f2508ade0a3 | 174 | //地上局へのGPS情報送信 |
zeutel | 7:1f2508ade0a3 | 175 | /*if(millis() - pretime > 5000){//5秒おきにGPS送信 |
zeutel | 7:1f2508ade0a3 | 176 | waitTime.start(); |
zeutel | 7:1f2508ade0a3 | 177 | waitTime.reset(); |
zeutel | 7:1f2508ade0a3 | 178 | while(gps.result==false){//GPSが利用可能になるまでまつ |
zeutel | 7:1f2508ade0a3 | 179 | gps.getgps(); |
zeutel | 7:1f2508ade0a3 | 180 | if(waitTime.read() > 1){ |
zeutel | 7:1f2508ade0a3 | 181 | waitTime.stop(); |
zeutel | 7:1f2508ade0a3 | 182 | break; |
zeutel | 7:1f2508ade0a3 | 183 | } |
zeutel | 7:1f2508ade0a3 | 184 | } |
zeutel | 7:1f2508ade0a3 | 185 | sprintf(sendData," %08.5fE, %08.5fN",gps.longitude,gps.latitude); |
zeutel | 7:1f2508ade0a3 | 186 | imSend(sendData); |
zeutel | 7:1f2508ade0a3 | 187 | }*/ |
zeutel | 7:1f2508ade0a3 | 188 | pretime=millis(); |
zeutel | 7:1f2508ade0a3 | 189 | } |
zeutel | 7:1f2508ade0a3 | 190 | } |
zeutel | 7:1f2508ade0a3 | 191 | |
zeutel | 7:1f2508ade0a3 | 192 | //着地後 |
zeutel | 7:1f2508ade0a3 | 193 | if(SDisAvailable){ |
zeutel | 7:1f2508ade0a3 | 194 | fclose(fp); |
zeutel | 7:1f2508ade0a3 | 195 | imSend("File closed"); |
zeutel | 7:1f2508ade0a3 | 196 | }else{ |
zeutel | 7:1f2508ade0a3 | 197 | imSend("SD error"); |
zeutel | 7:1f2508ade0a3 | 198 | } |
zeutel | 7:1f2508ade0a3 | 199 | |
zeutel | 7:1f2508ade0a3 | 200 | sprintf(sendData,"MaxAltitude:%06.2fm",maxAltitude); |
zeutel | 7:1f2508ade0a3 | 201 | imSend(sendData); |
zeutel | 7:1f2508ade0a3 | 202 | |
zeutel | 7:1f2508ade0a3 | 203 | while(1){ |
zeutel | 7:1f2508ade0a3 | 204 | imRecv(receive); |
zeutel | 7:1f2508ade0a3 | 205 | mbedOperate(receive); |
zeutel | 5:03d31f0a4943 | 206 | if(millis() - pretime > 5000){//5秒おきにGPS送信 |
zeutel | 5:03d31f0a4943 | 207 | if(sequence != 1){ |
zeutel | 5:03d31f0a4943 | 208 | waitTime.start(); |
zeutel | 5:03d31f0a4943 | 209 | waitTime.reset(); |
zeutel | 5:03d31f0a4943 | 210 | while(gps.result==false){//GPSが利用可能になるまでまつ |
zeutel | 5:03d31f0a4943 | 211 | gps.getgps(); |
zeutel | 5:03d31f0a4943 | 212 | if(waitTime.read() > 1){ |
zeutel | 5:03d31f0a4943 | 213 | waitTime.stop(); |
zeutel | 5:03d31f0a4943 | 214 | break; |
zeutel | 5:03d31f0a4943 | 215 | } |
Nozaryo | 3:1f93c4510fb1 | 216 | } |
zeutel | 7:1f2508ade0a3 | 217 | sprintf(sendData,"%08.5fE,%08.5fN",gps.longitude,gps.latitude); |
zeutel | 7:1f2508ade0a3 | 218 | imSend(sendData); |
zeutel | 5:03d31f0a4943 | 219 | } |
zeutel | 5:03d31f0a4943 | 220 | pretime=millis(); |
Nozaryo | 3:1f93c4510fb1 | 221 | } |
zeutel | 0:cebf1f73ffd5 | 222 | } |
zeutel | 0:cebf1f73ffd5 | 223 | } |
zeutel | 0:cebf1f73ffd5 | 224 | |
zeutel | 0:cebf1f73ffd5 | 225 | void init(FILE **file){ |
zeutel | 0:cebf1f73ffd5 | 226 | //im920 |
zeutel | 0:cebf1f73ffd5 | 227 | im920.init(); |
zeutel | 0:cebf1f73ffd5 | 228 | wait(1); |
zeutel | 0:cebf1f73ffd5 | 229 | |
zeutel | 0:cebf1f73ffd5 | 230 | //MMTXS03 |
zeutel | 0:cebf1f73ffd5 | 231 | if(lps331.isAvailable()) { |
zeutel | 7:1f2508ade0a3 | 232 | imSend("MM-TXS03,OK"); |
zeutel | 0:cebf1f73ffd5 | 233 | } else { |
zeutel | 7:1f2508ade0a3 | 234 | imSend("MM-TXS03,NG"); |
zeutel | 0:cebf1f73ffd5 | 235 | } |
zeutel | 0:cebf1f73ffd5 | 236 | |
zeutel | 0:cebf1f73ffd5 | 237 | //MM9DS1 |
zeutel | 7:1f2508ade0a3 | 238 | //lsm.begin(); |
zeutel | 7:1f2508ade0a3 | 239 | uint16_t WhoIsLsm = lsm.begin(); |
zeutel | 7:1f2508ade0a3 | 240 | if(WhoIsLsm==0x683D){ |
zeutel | 7:1f2508ade0a3 | 241 | imSend("MM-9DS1,OK"); |
zeutel | 7:1f2508ade0a3 | 242 | }else{ |
zeutel | 7:1f2508ade0a3 | 243 | imSend("MM-9DS1,NG"); |
zeutel | 7:1f2508ade0a3 | 244 | } |
zeutel | 7:1f2508ade0a3 | 245 | /*if(lsm.whoAmI()){ |
zeutel | 7:1f2508ade0a3 | 246 | imSend("MM-9DS1,OK"); |
zeutel | 0:cebf1f73ffd5 | 247 | }else { |
zeutel | 7:1f2508ade0a3 | 248 | imSend("MM-9DS1,NG"); |
zeutel | 7:1f2508ade0a3 | 249 | }*/ |
zeutel | 0:cebf1f73ffd5 | 250 | |
zeutel | 7:1f2508ade0a3 | 251 | //opening shock |
zeutel | 7:1f2508ade0a3 | 252 | if(accelerometer.getDeviceID()==0xE5) { |
zeutel | 7:1f2508ade0a3 | 253 | imSend("ADXL345,OK"); |
zeutel | 7:1f2508ade0a3 | 254 | }else{ |
zeutel | 7:1f2508ade0a3 | 255 | imSend("ADXL345,NG"); |
zeutel | 7:1f2508ade0a3 | 256 | } |
zeutel | 7:1f2508ade0a3 | 257 | |
Nozaryo | 3:1f93c4510fb1 | 258 | wait(1); |
zeutel | 5:03d31f0a4943 | 259 | |
zeutel | 0:cebf1f73ffd5 | 260 | //SD |
zeutel | 0:cebf1f73ffd5 | 261 | *file = fopen("/sd/data.csv", "w"); |
zeutel | 0:cebf1f73ffd5 | 262 | if(*file == NULL) { |
zeutel | 7:1f2508ade0a3 | 263 | imSend("SD,NG"); |
Nozaryo | 3:1f93c4510fb1 | 264 | SDisAvailable = false; |
zeutel | 0:cebf1f73ffd5 | 265 | }else{ |
zeutel | 7:1f2508ade0a3 | 266 | imSend("SD,OK"); |
zeutel | 5:03d31f0a4943 | 267 | 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 | 268 | SDisAvailable = true; |
zeutel | 0:cebf1f73ffd5 | 269 | } |
zeutel | 0:cebf1f73ffd5 | 270 | |
zeutel | 0:cebf1f73ffd5 | 271 | //GPS優先度 |
zeutel | 7:1f2508ade0a3 | 272 | /*NVIC_SetPriority(UART3_IRQn,0); |
zeutel | 0:cebf1f73ffd5 | 273 | gps.getgps(); |
zeutel | 0:cebf1f73ffd5 | 274 | if(gps.result){ |
zeutel | 7:1f2508ade0a3 | 275 | imSend("GPS...OK"); |
zeutel | 5:03d31f0a4943 | 276 | sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude); |
zeutel | 7:1f2508ade0a3 | 277 | imSend(sendData); |
zeutel | 0:cebf1f73ffd5 | 278 | }else{ |
zeutel | 7:1f2508ade0a3 | 279 | imSend("GPS...NG"); |
zeutel | 7:1f2508ade0a3 | 280 | }*/ |
zeutel | 0:cebf1f73ffd5 | 281 | |
zeutel | 0:cebf1f73ffd5 | 282 | //フライトピン |
Nozaryo | 2:cad76b5be4ba | 283 | //flightpin.output(); |
zeutel | 0:cebf1f73ffd5 | 284 | |
zeutel | 0:cebf1f73ffd5 | 285 | //各種設定 |
zeutel | 7:1f2508ade0a3 | 286 | lps331.setResolution(LPS331_I2C_PRESSURE_AVG_512, LPS331_I2C_TEMP_AVG_128); |
zeutel | 7:1f2508ade0a3 | 287 | lps331.setDataRate(LPS331_I2C_DATARATE_7HZ); |
zeutel | 0:cebf1f73ffd5 | 288 | lps331.setActive(true); |
zeutel | 0:cebf1f73ffd5 | 289 | lsm.setAccelODR(lsm.A_ODR_952); |
zeutel | 0:cebf1f73ffd5 | 290 | lsm.setGyroODR(lsm.G_ODR_952_BW_100); |
zeutel | 7:1f2508ade0a3 | 291 | lsm.setMagODR(lsm.M_ODR_80); |
zeutel | 7:1f2508ade0a3 | 292 | |
zeutel | 7:1f2508ade0a3 | 293 | // These are here to test whether any of the initialization fails. It will print the failure |
zeutel | 7:1f2508ade0a3 | 294 | /*if (accelerometer.setPowerControl(0x00)){ |
zeutel | 7:1f2508ade0a3 | 295 | pc.printf("didn't intitialize power control\r\n"); |
zeutel | 7:1f2508ade0a3 | 296 | } |
zeutel | 7:1f2508ade0a3 | 297 | //Full resolution, +/-16g, 4mg/LSB. |
zeutel | 7:1f2508ade0a3 | 298 | wait(.001); |
zeutel | 7:1f2508ade0a3 | 299 | |
zeutel | 7:1f2508ade0a3 | 300 | if(accelerometer.setDataFormatControl(0x0B)){ |
zeutel | 7:1f2508ade0a3 | 301 | pc.printf("didn't set data format\r\n"); |
zeutel | 7:1f2508ade0a3 | 302 | } |
zeutel | 7:1f2508ade0a3 | 303 | wait(.001);*/ |
zeutel | 7:1f2508ade0a3 | 304 | |
zeutel | 7:1f2508ade0a3 | 305 | //3.2kHz data rate. |
zeutel | 7:1f2508ade0a3 | 306 | if(accelerometer.setDataRate(ADXL345_200HZ)){ |
zeutel | 7:1f2508ade0a3 | 307 | pc.printf("didn't set data rate\r\n"); |
zeutel | 7:1f2508ade0a3 | 308 | } |
zeutel | 7:1f2508ade0a3 | 309 | wait(.001); |
zeutel | 7:1f2508ade0a3 | 310 | |
zeutel | 7:1f2508ade0a3 | 311 | //分解能 |
zeutel | 7:1f2508ade0a3 | 312 | accelerometer.setOpticalResolution(0x0B); |
zeutel | 7:1f2508ade0a3 | 313 | |
zeutel | 7:1f2508ade0a3 | 314 | //測定モード |
zeutel | 7:1f2508ade0a3 | 315 | if(accelerometer.setPowerControl(MeasurementMode)) { |
zeutel | 7:1f2508ade0a3 | 316 | pc.printf("didn't set the power control to measurement\r\n"); |
zeutel | 7:1f2508ade0a3 | 317 | } |
zeutel | 7:1f2508ade0a3 | 318 | |
zeutel | 7:1f2508ade0a3 | 319 | |
zeutel | 0:cebf1f73ffd5 | 320 | } |
zeutel | 0:cebf1f73ffd5 | 321 | |
zeutel | 0:cebf1f73ffd5 | 322 | void checkingSensors(){ |
zeutel | 0:cebf1f73ffd5 | 323 | //MMTXS03 |
zeutel | 0:cebf1f73ffd5 | 324 | if(lps331.isAvailable()) { |
zeutel | 7:1f2508ade0a3 | 325 | imSend("MM-TXS03,OK"); |
zeutel | 0:cebf1f73ffd5 | 326 | } else { |
zeutel | 7:1f2508ade0a3 | 327 | imSend("MM-TXS03,NG"); |
zeutel | 0:cebf1f73ffd5 | 328 | } |
zeutel | 0:cebf1f73ffd5 | 329 | |
zeutel | 0:cebf1f73ffd5 | 330 | //MM9DS1 |
zeutel | 0:cebf1f73ffd5 | 331 | if(lsm.whoAmI()){ |
zeutel | 7:1f2508ade0a3 | 332 | imSend("MM-9DS1,OK"); |
zeutel | 0:cebf1f73ffd5 | 333 | }else { |
zeutel | 7:1f2508ade0a3 | 334 | imSend("MM-9DS1,NG"); |
zeutel | 0:cebf1f73ffd5 | 335 | } |
zeutel | 0:cebf1f73ffd5 | 336 | |
zeutel | 7:1f2508ade0a3 | 337 | //opening shock |
zeutel | 7:1f2508ade0a3 | 338 | if(accelerometer.getDeviceID()==0xE5) { |
zeutel | 7:1f2508ade0a3 | 339 | imSend("ADXL345,OK"); |
zeutel | 7:1f2508ade0a3 | 340 | }else{ |
zeutel | 7:1f2508ade0a3 | 341 | imSend("ADXL345,NG"); |
zeutel | 7:1f2508ade0a3 | 342 | } |
zeutel | 7:1f2508ade0a3 | 343 | |
zeutel | 0:cebf1f73ffd5 | 344 | //GPS |
zeutel | 7:1f2508ade0a3 | 345 | /*gps.getgps(); |
zeutel | 0:cebf1f73ffd5 | 346 | if(gps.result){ |
zeutel | 7:1f2508ade0a3 | 347 | imSend("GPS...OK"); |
zeutel | 5:03d31f0a4943 | 348 | sprintf(sendData,"%05.2f,%04.2f",gps.longitude,gps.latitude); |
zeutel | 7:1f2508ade0a3 | 349 | imSend(sendData); |
zeutel | 0:cebf1f73ffd5 | 350 | }else{ |
zeutel | 7:1f2508ade0a3 | 351 | imSend("GPS...NG"); |
zeutel | 7:1f2508ade0a3 | 352 | }*/ |
zeutel | 0:cebf1f73ffd5 | 353 | } |
zeutel | 0:cebf1f73ffd5 | 354 | |
zeutel | 7:1f2508ade0a3 | 355 | void readValues(int sequence,float *temperature,float *pressure,float *openingShockReadings){ |
zeutel | 7:1f2508ade0a3 | 356 | int accels[3]; |
zeutel | 7:1f2508ade0a3 | 357 | |
zeutel | 0:cebf1f73ffd5 | 358 | lsm.readAccel(); |
zeutel | 0:cebf1f73ffd5 | 359 | lsm.readGyro(); |
zeutel | 0:cebf1f73ffd5 | 360 | lsm.readMag(); |
zeutel | 7:1f2508ade0a3 | 361 | *pressure=lps331.getPressure(); |
zeutel | 7:1f2508ade0a3 | 362 | *temperature=lps331.getTemperature(); |
zeutel | 7:1f2508ade0a3 | 363 | /*if(sequence!=1){ |
zeutel | 0:cebf1f73ffd5 | 364 | gps.getgps(); |
zeutel | 7:1f2508ade0a3 | 365 | }*/ |
zeutel | 7:1f2508ade0a3 | 366 | accelerometer.getOutput(accels); |
zeutel | 7:1f2508ade0a3 | 367 | for(int i=0;i<3;i++){ |
zeutel | 7:1f2508ade0a3 | 368 | openingShockReadings[i]=(int16_t)accels[i];//*0.0392266; |
zeutel | 0:cebf1f73ffd5 | 369 | } |
zeutel | 0:cebf1f73ffd5 | 370 | } |
zeutel | 0:cebf1f73ffd5 | 371 | |
zeutel | 0:cebf1f73ffd5 | 372 | float calcAltitude(float pres, float temp){ |
zeutel | 7:1f2508ade0a3 | 373 | return (pow(1015.5/pres,1/5.257)-1)*(temp+273.15)/0.0065; |
zeutel | 7:1f2508ade0a3 | 374 | } |
zeutel | 7:1f2508ade0a3 | 375 | |
zeutel | 7:1f2508ade0a3 | 376 | void imRecv(char *receive){ |
zeutel | 7:1f2508ade0a3 | 377 | im920.poll(); |
zeutel | 7:1f2508ade0a3 | 378 | memset(receive,'\0',sizeof(receive)); |
zeutel | 7:1f2508ade0a3 | 379 | im920.recv(receive,8); |
zeutel | 7:1f2508ade0a3 | 380 | pc.printf(receive); |
zeutel | 7:1f2508ade0a3 | 381 | //camera用コマンド認識 |
zeutel | 7:1f2508ade0a3 | 382 | char *camStr=strtok(receive,":");//cam:startというようにコマンド |
zeutel | 7:1f2508ade0a3 | 383 | if(strncmp(camStr,"cam",sizeof(camStr))==0){ |
zeutel | 7:1f2508ade0a3 | 384 | imSend(receive); |
zeutel | 7:1f2508ade0a3 | 385 | } |
zeutel | 7:1f2508ade0a3 | 386 | } |
zeutel | 7:1f2508ade0a3 | 387 | |
zeutel | 7:1f2508ade0a3 | 388 | void imSend(char *send){ |
zeutel | 7:1f2508ade0a3 | 389 | im920.send(send,strlen(send)+1); |
zeutel | 7:1f2508ade0a3 | 390 | pc.printf(send); |
zeutel | 7:1f2508ade0a3 | 391 | pc.printf("\r\n"); |
zeutel | 7:1f2508ade0a3 | 392 | } |
zeutel | 7:1f2508ade0a3 | 393 | |
zeutel | 7:1f2508ade0a3 | 394 | void mbedOperate(char *command){ |
zeutel | 7:1f2508ade0a3 | 395 | if(strncmp(command,"reset",sizeof(command))==0){ |
zeutel | 7:1f2508ade0a3 | 396 | imSend("Mbed reset"); |
zeutel | 7:1f2508ade0a3 | 397 | NVIC_SystemReset(); |
zeutel | 7:1f2508ade0a3 | 398 | } |
zeutel | 7:1f2508ade0a3 | 399 | |
zeutel | 7:1f2508ade0a3 | 400 | if(strncmp(command,"retry",sizeof(command))==0){ |
zeutel | 7:1f2508ade0a3 | 401 | imSend("Retry initializing"); |
zeutel | 7:1f2508ade0a3 | 402 | init(&fp); |
zeutel | 7:1f2508ade0a3 | 403 | wait(1); |
zeutel | 7:1f2508ade0a3 | 404 | } |
zeutel | 7:1f2508ade0a3 | 405 | |
zeutel | 7:1f2508ade0a3 | 406 | if(strncmp(command,"check",sizeof(command))==0){ |
zeutel | 7:1f2508ade0a3 | 407 | imSend("Check"); |
zeutel | 7:1f2508ade0a3 | 408 | checkingSensors(); |
zeutel | 7:1f2508ade0a3 | 409 | } |
zeutel | 7:1f2508ade0a3 | 410 | } |
zeutel | 7:1f2508ade0a3 | 411 | |
zeutel | 7:1f2508ade0a3 | 412 | void sendDatas(float time, float temp, float pres, float ax, float ay, float az, float gx, float gy, float gz){ |
zeutel | 7:1f2508ade0a3 | 413 | sprintf(sendData,"data1,%.3f,%.3f,%.3f",time,temp,pres); |
zeutel | 7:1f2508ade0a3 | 414 | imSend(sendData); |
zeutel | 7:1f2508ade0a3 | 415 | sprintf(sendData,"data2,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f",ax,ay,az,gx,gy,gz); |
zeutel | 7:1f2508ade0a3 | 416 | imSend(sendData); |
zeutel | 0:cebf1f73ffd5 | 417 | } |