FTE-denshi / Mbed 2 deprecated Izu_2019

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?

UserRevisionLine numberNew 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 }