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