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      }