IMU-pressure-tempreture sensors
Dependencies: CMSIS_DSP_401 DHT22 MPU9150_DMP QuaternionMath MODSERIAL mbed-src FATFileSystem111 SDFileSystem11 Camera_LS_Y201_CANSAT
main.cpp
00001 #include "MPU9150.h" 00002 #include "Quaternion.h" 00003 #include "BMP085.h" 00004 #include "DHT22.h" 00005 #include "main.h" 00006 00007 00008 #include "Camera_LS_Y201.h" 00009 #include "SDFileSystem.h" 00010 char ns, ew, tf, status; 00011 int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix 00012 float latitude, longitude, timefix, speed, altitude; 00013 Serial xbee(p28,p27); 00014 00015 00016 DHT22 dht22(p23); 00017 BMP085 bmp085(p9, p10,BMP085_oss8); 00018 00019 00020 //DigitalOut myled(LED1); 00021 00022 Serial debug(USBTX, USBRX); 00023 MPU9150 imu(p10, p9, p15); 00024 ///////////////////////////////////////////////////// 00025 #define DEBMSG debug.printf 00026 #define NEWLINE() debug.printf("\r\n") 00027 00028 #define USE_SDCARD 0 00029 00030 #if USE_SDCARD 00031 #define FILENAME "/sd/IMG_%04d.jpg" 00032 SDFileSystem fs(p5, p6, p7, p8, "sd"); 00033 #else 00034 #define FILENAME "/local/IMG_%04d.jpg" 00035 LocalFileSystem fs("local"); 00036 #endif 00037 Camera_LS_Y201 cam1(p17, p18); 00038 00039 int pow(int x,int y) 00040 { 00041 int z=x; 00042 for ( i=1;i<y;i++) 00043 { 00044 z *=x; 00045 } 00046 00047 return z; 00048 } 00049 typedef struct work { 00050 FILE *fp; 00051 } work_t; 00052 00053 work_t work; 00054 void converter(double x) 00055 { 00056 int i; 00057 x*=pow(10,6); 00058 int z; 00059 for(i=9;i>=0;i--) 00060 { 00061 if(i==5){ 00062 xbee.printf("."); 00063 wait(0.02); 00064 } 00065 z = x/(pow(10,i)); 00066 x =x-(z*pow(10,i)); 00067 xbee.printf("%d",z); 00068 wait(0.02); 00069 } 00070 00071 } 00072 00073 00074 uint8_t send[16] = { 00075 0x56, 00076 0x00, 00077 0x32, 00078 0x0C, 00079 0x00, 00080 0x0A, 00081 0x00, 00082 0x00, 00083 0x00, // MH 00084 0x00, // ML 00085 0x00, 00086 0x00, 00087 0x00, // KH 00088 0x00, // KL 00089 0x00, // XX 00090 0x00 // XX 00091 }; 00092 00093 uint16_t x = 10; // Interval time. XX XX * 0.01m[sec] 00094 bool end = true; 00095 uint16_t m = 0; // Staring address. 00096 /* 00097 * Get the data size. 00098 */ 00099 uint8_t body[16000]; 00100 uint16_t k = sizeof(body); 00101 int siz_done = 0; 00102 int siz_total = 0; 00103 int cnt = 0; 00104 int z; 00105 ////////////////////////////////////////// 00106 void callback_func(int done, int total, uint8_t *buf, size_t siz) { 00107 00108 fwrite(buf, siz, 1, work.fp); 00109 00110 //for(int i=0;i<siz;i++) 00111 //{ 00112 //xbee.printf("%x",buf[i]); 00113 //fprintf(work.fp,"%c",buf[i]); 00114 //} 00115 00116 static int n = 0; 00117 int tmp = done * 100 / total; 00118 if (n != tmp) { 00119 n = tmp; 00120 DEBMSG("Writ%3d%%", n); 00121 NEWLINE(); 00122 } 00123 } 00124 00125 00126 00127 //////////////////////////////////////////// 00128 00129 00130 00131 00132 void readdJpegFileContent(void (*func)(int done, int total, uint8_t *buf, size_t siz)) { 00133 00134 00135 00136 if(m==0) 00137 { 00138 z=sizeof(body); 00139 } 00140 00141 //if (r != NoError) { 00142 // return r; 00143 //} 00144 00145 00146 send[8] = (m >> 8) & 0xff; 00147 send[9] = (m >> 0) & 0xff; 00148 send[12] = (k >> 8) & 0xff; 00149 send[13] = (k >> 0) & 0xff; 00150 send[14] = (x >> 8) & 0xff; 00151 send[15] = (x >> 0) & 0xff; 00152 /* 00153 * Send a command. 00154 */ 00155 cam1.sendBytes(send, sizeof(send), 200 * 1000) ; 00156 // printf("sended header"); 00157 // return SendError; 00158 00159 /* 00160 * Read the header of the response. 00161 */ 00162 uint8_t header[5]; 00163 cam1.recvBytes(header, sizeof(header), 2 * 1000 * 1000); 00164 // return RecvError; 00165 //printf("recved header"); 00166 /* 00167 * Check the response and fetch an image data. 00168 */ 00169 00170 if ((header[0] == 0x76) 00171 && (header[1] == 0x00) 00172 && (header[2] == 0x32) 00173 && (header[3] == 0x00) 00174 && (header[4] == 0x00)) { 00175 00176 cam1.recvBytes(body, z, 2 * 1000 * 1000); //{ 00177 //return RecvError; 00178 // } 00179 // printf("saved"); 00180 00181 siz_done += z; 00182 00183 if (func != NULL) { 00184 if (siz_done > siz_total) { 00185 z=sizeof(body)-siz_done+siz_total+3; 00186 siz_done = siz_total; 00187 00188 00189 } 00190 func(siz_done, siz_total, body, z); 00191 } 00192 for (int i = 1; i < z; i++) { 00193 if ((body[i - 1] == 0xFF) && (body[i - 0] == 0xD9)) { 00194 end = true; 00195 //printf("saved%d",i); 00196 } 00197 } 00198 } //else { 00199 // return UnexpectedReply; 00200 //} 00201 /* 00202 * Read the footer of the response. 00203 */ 00204 uint8_t footer[5]; 00205 cam1.recvBytes(footer, sizeof(footer), 2 * 1000 * 1000);// { 00206 // return RecvError; 00207 //} 00208 00209 m += z; 00210 00211 // return NoError; 00212 } 00213 00214 ////////////////// 00215 /** 00216 * Capture. 00217 * 00218 * @param cam A pointer to a camera object. 00219 * @param filename The file name. 00220 * 00221 * @return Return 0 if it succeed. 00222 */ 00223 int capture(Camera_LS_Y201 *cam, char *filename) { 00224 /* 00225 * Take a picture. 00226 */ 00227 if(end==true) 00228 { 00229 if (cam->takePicture() != 0) { 00230 return -1; 00231 } 00232 cam1.readJpegFileSize(&siz_total); 00233 // printf("%d",siz_total); 00234 siz_done = 0; 00235 00236 00237 DEBMSG("Captured."); 00238 NEWLINE(); 00239 work.fp = fopen(filename, "wb"); 00240 //p1= fopen("/sd/IMG_11111.txt", "wb"); 00241 if (work.fp == NULL) { 00242 return -2; 00243 } 00244 00245 /* 00246 * Read the content. 00247 */ 00248 DEBMSG("%s", filename); 00249 NEWLINE(); 00250 end=false; 00251 } 00252 /* 00253 * Open file. 00254 */ 00255 00256 readdJpegFileContent(callback_func); 00257 // fclose(work.fp); 00258 //return -3; 00259 00260 00261 00262 /* 00263 * Stop taking pictures. 00264 */ 00265 if(end== true) 00266 { 00267 fclose(work.fp); 00268 cam->stopTakingPictures(); 00269 00270 DEBMSG("[%04d]:OK.", cnt); 00271 NEWLINE(); 00272 m=0; 00273 cnt++; 00274 //wait(1); 00275 //xbee.printf("saved"); 00276 } 00277 return 0; 00278 } 00279 ////////////////////////////////////////////////////// 00280 DigitalOut led(LED1); 00281 void Init() 00282 { 00283 gps.baud(9600); 00284 debug.baud(115200); 00285 00286 // xbee.printf("Init OK\n"); 00287 printf("Init OK\n"); 00288 } 00289 00290 char buffer[200]; 00291 int e=6; 00292 int n=0; 00293 int main(){ 00294 xbee.baud(115200); 00295 pc.baud(115200); 00296 00297 DEBMSG("Camera module"); 00298 NEWLINE(); 00299 DEBMSG("Resetting..."); 00300 NEWLINE(); 00301 wait(1); 00302 00303 if (cam1.reset() == 0) { 00304 DEBMSG("Reset OK."); 00305 NEWLINE(); 00306 } else { 00307 DEBMSG("Reset fail."); 00308 NEWLINE(); 00309 error("Reset fail."); 00310 } 00311 00312 00313 wait(1); 00314 cam1.baud(); 00315 int j=1; 00316 int g=1; 00317 Init(); 00318 char c; 00319 //debug.baud(115200); 00320 00321 if(imu.isReady()){ 00322 //xbee.printf("MPU9150 is ready\r\n"); 00323 printf("MPU9150 is ready\r\n"); 00324 } else { 00325 //xbee.printf("MPU9150 initialisation failure\r\n"); 00326 printf("MPU9150 initialisation failure\r\n"); 00327 } 00328 00329 imu.initialiseDMP(); 00330 00331 Timer timer; 00332 timer.start(); 00333 00334 imu.setFifoReset(true); 00335 imu.setDMPEnabled(true); 00336 00337 Quaternion q1; 00338 00339 float hum,temp; 00340 // int g=10; 00341 00342 while(true){ 00343 Timer timer1; 00344 timer1.start(); 00345 // n++; 00346 // wait(0.5); 00347 bmp085.update(); 00348 float allltitude=bmp085.calcAltitude(bmp085.get_pressure()*100); 00349 //wait(0.2); 00350 //if(e==6) 00351 //{ 00352 dht22.sample() ; 00353 hum=dht22.getHumidity()/10.0; 00354 temp=dht22.getTemperature()/10.0; 00355 00356 //e=1; 00357 //} 00358 00359 00360 if(imu.getFifoCount() >= 48){ 00361 imu.getFifoBuffer(buffer, 48); 00362 led = !led; 00363 } 00364 // debug.printf("vcvssgsgf"); 00365 if(timer.read_ms() >50){ 00366 timer.reset(); 00367 00368 //This is the format of the data in the fifo, 00369 /* ================================================================================================ * 00370 | Default MotionApps v4.1 48-byte FIFO packet structure: | 00371 | | 00372 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 00373 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 00374 | | 00375 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 00376 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | 00377 * ================================================================================================ */ 00378 00379 /* 00380 debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[34] << 24) + ((int32_t)buffer[35] << 16) + ((int32_t)buffer[36] << 8) + (int32_t)buffer[37]), 00381 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), 00382 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45])); 00383 00384 debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[16] << 24) + ((int32_t)buffer[17] << 16) + ((int32_t)buffer[18] << 8) + (int32_t)buffer[19]), 00385 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]), 00386 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27])); 00387 00388 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28], 00389 (int16_t)(buffer[31] << 8) + buffer[30], 00390 (int16_t)(buffer[33] << 8) + buffer[32]); 00391 00392 debug.printf("%f, %f, %f, %f\r\n", 00393 (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), 00394 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), 00395 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), 00396 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); 00397 */ 00398 00399 q1.decode(buffer); 00400 00401 //wait(0.5); 00402 00403 if(gps.readable()) 00404 { 00405 if(gps.getc() == '$'); // wait a $ 00406 { 00407 for(int i=0; i<sizeof(cDataBuffer); i++) 00408 { 00409 c = gps.getc(); 00410 if( c == '\r' ) 00411 { 00412 //pc.printf("%s\n", cDataBuffer); 00413 parse(cDataBuffer, i); 00414 i = sizeof(cDataBuffer); 00415 } 00416 else 00417 { 00418 cDataBuffer[i] = c; 00419 } 00420 } 00421 } 00422 } 00423 00424 00425 // if(n==10) 00426 //{ 00427 // xbee.printf("w:%f, v.x:%f, v.y:%f, v.z:%f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z); 00428 xbee.printf(","); 00429 wait(0.02); 00430 xbee.printf("p"); 00431 wait(0.02); 00432 float x =bmp085.get_pressure(); 00433 converter( x); 00434 wait(0.02); 00435 xbee.printf(","); 00436 wait(0.02); 00437 xbee.printf("t"); 00438 wait(0.02); 00439 x =(bmp085.get_temperature()+temp)/2; 00440 00441 00442 converter( x); 00443 wait(0.02); 00444 xbee.printf(","); 00445 wait(0.02); 00446 xbee.printf("h"); 00447 wait(0.02); 00448 x=hum; 00449 converter( x); 00450 wait(0.02); 00451 xbee.printf(","); 00452 wait(0.02); 00453 xbee.printf("A"); //A for altitude from pressure sensor 00454 wait(0.02); 00455 x =allltitude; 00456 00457 converter( x); 00458 //imu variables q1.w, q1.v.x, q1.v.y, q1.v.z 00459 00460 wait(0.02); 00461 xbee.printf(","); 00462 wait(0.02); 00463 xbee.printf("w"); 00464 wait(0.02); 00465 x =q1.w; 00466 00467 converter( x); 00468 wait(0.02); 00469 xbee.printf(","); 00470 wait(0.02); 00471 xbee.printf("x"); 00472 wait(0.02); 00473 x =q1.v.x; 00474 00475 00476 converter( x); 00477 wait(0.02); 00478 xbee.printf(","); 00479 wait(0.02); 00480 xbee.printf("y"); 00481 wait(0.02); 00482 x =q1.v.y; 00483 converter(x); 00484 wait(0.02); 00485 xbee.printf(","); 00486 wait(0.02); 00487 xbee.printf("z"); 00488 wait(0.02); 00489 x =q1.v.z; 00490 converter(x); 00491 // gps variables latitude,longitude 00492 wait(0.02); 00493 xbee.printf(","); 00494 wait(0.02); 00495 xbee.printf("a"); 00496 wait(0.02); 00497 x = latitude; 00498 converter(x); 00499 wait(0.02); 00500 xbee.printf(","); 00501 wait(0.02); 00502 xbee.printf("o"); 00503 wait(0.02); 00504 x = longitude; 00505 converter(x); 00506 //xbee.printf("p:%f hPa / t:%f / altitude=%f \n\r",bmp085.get_pressure() , bmp085.get_temperature(),allltitude); 00507 00508 // xbee.printf("temp: %f , hum:%f \n\r",temp,hum); 00509 00510 00511 // xbee.printf("a: %f, o: %f", latitude,longitude); 00512 //////////////////////////////////////////////////////// 00513 //xbee.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); 00514 // xbee.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); 00515 // xbee.printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n\n\r\n\n\n", timefix, status, latitude, ns, longitude, ew, speed, date); 00516 //n=1; 00517 00518 //} 00519 00520 00521 printf("w:%f, v.x:%f, v.y:%f, v.z:%f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z); 00522 printf("p:%f hPa / t:%f / altitude=%f \n\r",x , bmp085.get_temperature(),allltitude); 00523 printf("temp: %f , hum:%f \n\r",temp,hum); 00524 printf("GPGGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude); 00525 printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); 00526 printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); 00527 printf("GPRMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Date: %d\n\n\r\n\n\n", timefix, status, latitude, ns, longitude, ew, speed, date); 00528 00529 char fname[64]; 00530 // xbee.printf("hello%d\n\r",g); 00531 00532 if((end==true)||(j==1)) 00533 { 00534 00535 snprintf(fname, sizeof(fname) - 1, FILENAME, cnt); 00536 j=0; 00537 } 00538 capture(&cam1, fname); 00539 //if (r == 0) { 00540 00541 00542 // } 00543 //} else { 00544 //DEBMSG("[%04d]:NG. (code=%d)", cnt, r); 00545 // NEWLINE(); 00546 // error("Failure."); 00547 //} 00548 g++; 00549 00550 00551 00552 imu.setFifoReset(true); 00553 imu.setDMPEnabled(true); 00554 //TeaPot Demo Packet for MotionFit SDK 00555 /* 00556 debug.putc('$'); //packet start 00557 debug.putc((char)2); //assume packet type constant 00558 debug.putc((char)0); //count seems unused 00559 for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats 00560 debug.putc((char)(buffer[i])); 00561 } 00562 for(int i = 0; i < 5; i++){ //no idea, padded with 0 00563 debug.putc((char)0); 00564 } 00565 */ 00566 } 00567 00568 while(1) 00569 { 00570 if(timer1.read_ms()>=2000) 00571 { 00572 timer1.reset(); 00573 break; 00574 } 00575 } 00576 00577 //e++; 00578 //g++; 00579 //xbee.printf("this is the end"); 00580 //rst=1; 00581 //wait(0.001); 00582 //rst=0; 00583 } 00584 00585 } 00586 00587 00588 00589 00590 void parse(char *cmd, int n) 00591 { 00592 00593 00594 00595 00596 // Global Positioning System Fix Data 00597 if(strncmp(cmd,"$GPGGA", 6) == 0) 00598 { 00599 sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude); 00600 00601 } 00602 00603 // Satellite status 00604 if(strncmp(cmd,"$GPGSA", 6) == 0) 00605 { 00606 sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst); 00607 00608 } 00609 00610 // Geographic position, Latitude and Longitude 00611 if(strncmp(cmd,"$GPGLL", 6) == 0) 00612 { 00613 sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix); 00614 00615 } 00616 00617 // Geographic position, Latitude and Longitude 00618 if(strncmp(cmd,"$GPRMC", 6) == 0) 00619 { 00620 sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date); 00621 00622 } 00623 00624 }
Generated on Sun Jul 17 2022 18:03:01 by 1.7.2