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);
        
    }
       
     }