IMU-pressure-tempreture sensors
Dependencies: CMSIS_DSP_401 DHT22 MPU9150_DMP QuaternionMath MODSERIAL mbed-src FATFileSystem111 SDFileSystem11 Camera_LS_Y201_CANSAT
main.cpp
- Committer:
- Hagrass
- Date:
- 2015-09-09
- Revision:
- 4:dbb8e901826d
- Parent:
- 1:339ebc8786ca
File content as of revision 4:dbb8e901826d:
#include "MPU9150.h" #include "Quaternion.h" #include "BMP085.h" #include "DHT22.h" #include "main.h" #include "Camera_LS_Y201.h" #include "SDFileSystem.h" char ns, ew, tf, status; int fq, nst, fix, date; // fix quality, Number of satellites being tracked, 3D fix float latitude, longitude, timefix, speed, altitude; Serial xbee(p28,p27); DHT22 dht22(p23); BMP085 bmp085(p9, p10,BMP085_oss8); //DigitalOut myled(LED1); Serial debug(USBTX, USBRX); MPU9150 imu(p10, p9, p15); ///////////////////////////////////////////////////// #define DEBMSG debug.printf #define NEWLINE() debug.printf("\r\n") #define USE_SDCARD 0 #if USE_SDCARD #define FILENAME "/sd/IMG_%04d.jpg" SDFileSystem fs(p5, p6, p7, p8, "sd"); #else #define FILENAME "/local/IMG_%04d.jpg" LocalFileSystem fs("local"); #endif Camera_LS_Y201 cam1(p17, p18); int pow(int x,int y) { int z=x; for ( i=1;i<y;i++) { z *=x; } return z; } typedef struct work { FILE *fp; } work_t; work_t work; void converter(double x) { int i; x*=pow(10,6); int z; for(i=9;i>=0;i--) { if(i==5){ xbee.printf("."); wait(0.02); } z = x/(pow(10,i)); x =x-(z*pow(10,i)); xbee.printf("%d",z); wait(0.02); } } uint8_t send[16] = { 0x56, 0x00, 0x32, 0x0C, 0x00, 0x0A, 0x00, 0x00, 0x00, // MH 0x00, // ML 0x00, 0x00, 0x00, // KH 0x00, // KL 0x00, // XX 0x00 // XX }; uint16_t x = 10; // Interval time. XX XX * 0.01m[sec] bool end = true; uint16_t m = 0; // Staring address. /* * Get the data size. */ uint8_t body[16000]; uint16_t k = sizeof(body); int siz_done = 0; int siz_total = 0; int cnt = 0; int z; ////////////////////////////////////////// void callback_func(int done, int total, uint8_t *buf, size_t siz) { fwrite(buf, siz, 1, work.fp); //for(int i=0;i<siz;i++) //{ //xbee.printf("%x",buf[i]); //fprintf(work.fp,"%c",buf[i]); //} static int n = 0; int tmp = done * 100 / total; if (n != tmp) { n = tmp; DEBMSG("Writ%3d%%", n); NEWLINE(); } } //////////////////////////////////////////// void readdJpegFileContent(void (*func)(int done, int total, uint8_t *buf, size_t siz)) { if(m==0) { z=sizeof(body); } //if (r != NoError) { // return r; //} send[8] = (m >> 8) & 0xff; send[9] = (m >> 0) & 0xff; send[12] = (k >> 8) & 0xff; send[13] = (k >> 0) & 0xff; send[14] = (x >> 8) & 0xff; send[15] = (x >> 0) & 0xff; /* * Send a command. */ cam1.sendBytes(send, sizeof(send), 200 * 1000) ; // printf("sended header"); // return SendError; /* * Read the header of the response. */ uint8_t header[5]; cam1.recvBytes(header, sizeof(header), 2 * 1000 * 1000); // return RecvError; //printf("recved header"); /* * Check the response and fetch an image data. */ if ((header[0] == 0x76) && (header[1] == 0x00) && (header[2] == 0x32) && (header[3] == 0x00) && (header[4] == 0x00)) { cam1.recvBytes(body, z, 2 * 1000 * 1000); //{ //return RecvError; // } // printf("saved"); siz_done += z; if (func != NULL) { if (siz_done > siz_total) { z=sizeof(body)-siz_done+siz_total+3; siz_done = siz_total; } func(siz_done, siz_total, body, z); } for (int i = 1; i < z; i++) { if ((body[i - 1] == 0xFF) && (body[i - 0] == 0xD9)) { end = true; //printf("saved%d",i); } } } //else { // return UnexpectedReply; //} /* * Read the footer of the response. */ uint8_t footer[5]; cam1.recvBytes(footer, sizeof(footer), 2 * 1000 * 1000);// { // return RecvError; //} m += z; // return NoError; } ////////////////// /** * Capture. * * @param cam A pointer to a camera object. * @param filename The file name. * * @return Return 0 if it succeed. */ int capture(Camera_LS_Y201 *cam, char *filename) { /* * Take a picture. */ if(end==true) { if (cam->takePicture() != 0) { return -1; } cam1.readJpegFileSize(&siz_total); // printf("%d",siz_total); siz_done = 0; DEBMSG("Captured."); NEWLINE(); work.fp = fopen(filename, "wb"); //p1= fopen("/sd/IMG_11111.txt", "wb"); if (work.fp == NULL) { return -2; } /* * Read the content. */ DEBMSG("%s", filename); NEWLINE(); end=false; } /* * Open file. */ readdJpegFileContent(callback_func); // fclose(work.fp); //return -3; /* * Stop taking pictures. */ if(end== true) { fclose(work.fp); cam->stopTakingPictures(); DEBMSG("[%04d]:OK.", cnt); NEWLINE(); m=0; cnt++; //wait(1); //xbee.printf("saved"); } return 0; } ////////////////////////////////////////////////////// DigitalOut led(LED1); void Init() { gps.baud(9600); debug.baud(115200); // xbee.printf("Init OK\n"); printf("Init OK\n"); } char buffer[200]; int e=6; int n=0; int main(){ xbee.baud(115200); pc.baud(115200); DEBMSG("Camera module"); NEWLINE(); DEBMSG("Resetting..."); NEWLINE(); wait(1); if (cam1.reset() == 0) { DEBMSG("Reset OK."); NEWLINE(); } else { DEBMSG("Reset fail."); NEWLINE(); error("Reset fail."); } wait(1); cam1.baud(); int j=1; int g=1; Init(); char c; //debug.baud(115200); if(imu.isReady()){ //xbee.printf("MPU9150 is ready\r\n"); printf("MPU9150 is ready\r\n"); } else { //xbee.printf("MPU9150 initialisation failure\r\n"); printf("MPU9150 initialisation failure\r\n"); } imu.initialiseDMP(); Timer timer; timer.start(); imu.setFifoReset(true); imu.setDMPEnabled(true); Quaternion q1; float hum,temp; // int g=10; while(true){ Timer timer1; timer1.start(); // n++; // wait(0.5); bmp085.update(); float allltitude=bmp085.calcAltitude(bmp085.get_pressure()*100); //wait(0.2); //if(e==6) //{ dht22.sample() ; hum=dht22.getHumidity()/10.0; temp=dht22.getTemperature()/10.0; //e=1; //} if(imu.getFifoCount() >= 48){ imu.getFifoBuffer(buffer, 48); led = !led; } // debug.printf("vcvssgsgf"); if(timer.read_ms() >50){ timer.reset(); //This is the format of the data in the fifo, /* ================================================================================================ * | Default MotionApps v4.1 48-byte FIFO packet structure: | | | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | | | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | * ================================================================================================ */ /* 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]), (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]), (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45])); 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]), (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]), (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27])); debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28], (int16_t)(buffer[31] << 8) + buffer[30], (int16_t)(buffer[33] << 8) + buffer[32]); debug.printf("%f, %f, %f, %f\r\n", (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)), (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30))); */ q1.decode(buffer); //wait(0.5); if(gps.readable()) { if(gps.getc() == '$'); // wait a $ { for(int i=0; i<sizeof(cDataBuffer); i++) { c = gps.getc(); if( c == '\r' ) { //pc.printf("%s\n", cDataBuffer); parse(cDataBuffer, i); i = sizeof(cDataBuffer); } else { cDataBuffer[i] = c; } } } } // if(n==10) //{ // 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); xbee.printf(","); wait(0.02); xbee.printf("p"); wait(0.02); float x =bmp085.get_pressure(); converter( x); wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("t"); wait(0.02); x =(bmp085.get_temperature()+temp)/2; converter( x); wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("h"); wait(0.02); x=hum; converter( x); wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("A"); //A for altitude from pressure sensor wait(0.02); x =allltitude; converter( x); //imu variables q1.w, q1.v.x, q1.v.y, q1.v.z wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("w"); wait(0.02); x =q1.w; converter( x); wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("x"); wait(0.02); x =q1.v.x; converter( x); wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("y"); wait(0.02); x =q1.v.y; converter(x); wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("z"); wait(0.02); x =q1.v.z; converter(x); // gps variables latitude,longitude wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("a"); wait(0.02); x = latitude; converter(x); wait(0.02); xbee.printf(","); wait(0.02); xbee.printf("o"); wait(0.02); x = longitude; converter(x); //xbee.printf("p:%f hPa / t:%f / altitude=%f \n\r",bmp085.get_pressure() , bmp085.get_temperature(),allltitude); // xbee.printf("temp: %f , hum:%f \n\r",temp,hum); // xbee.printf("a: %f, o: %f", latitude,longitude); //////////////////////////////////////////////////////// //xbee.printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); // xbee.printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); // 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); //n=1; //} 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); printf("p:%f hPa / t:%f / altitude=%f \n\r",x , bmp085.get_temperature(),allltitude); printf("temp: %f , hum:%f \n\r",temp,hum); 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); printf("GPGSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst); printf("GPGLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\n", latitude, ns, longitude, ew, timefix); 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); char fname[64]; // xbee.printf("hello%d\n\r",g); if((end==true)||(j==1)) { snprintf(fname, sizeof(fname) - 1, FILENAME, cnt); j=0; } capture(&cam1, fname); //if (r == 0) { // } //} else { //DEBMSG("[%04d]:NG. (code=%d)", cnt, r); // NEWLINE(); // error("Failure."); //} g++; imu.setFifoReset(true); imu.setDMPEnabled(true); //TeaPot Demo Packet for MotionFit SDK /* debug.putc('$'); //packet start debug.putc((char)2); //assume packet type constant debug.putc((char)0); //count seems unused for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats debug.putc((char)(buffer[i])); } for(int i = 0; i < 5; i++){ //no idea, padded with 0 debug.putc((char)0); } */ } while(1) { if(timer1.read_ms()>=2000) { timer1.reset(); break; } } //e++; //g++; //xbee.printf("this is the end"); //rst=1; //wait(0.001); //rst=0; } } void parse(char *cmd, int n) { // Global Positioning System Fix Data if(strncmp(cmd,"$GPGGA", 6) == 0) { sscanf(cmd, "$GPGGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &timefix, &latitude, &ns, &longitude, &ew, &fq, &nst, &altitude); } // Satellite status if(strncmp(cmd,"$GPGSA", 6) == 0) { sscanf(cmd, "$GPGSA,%c,%d,%d", &tf, &fix, &nst); } // Geographic position, Latitude and Longitude if(strncmp(cmd,"$GPGLL", 6) == 0) { sscanf(cmd, "$GPGLL,%f,%c,%f,%c,%f", &latitude, &ns, &longitude, &ew, &timefix); } // Geographic position, Latitude and Longitude if(strncmp(cmd,"$GPRMC", 6) == 0) { sscanf(cmd, "$GPRMC,%f,%c,%f,%c,%f,%c,%f,,%d", &timefix, &status, &latitude, &ns, &longitude, &ew, &speed, &date); } }