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