File content as of revision 15:e3e03a9df89e:

#include "handleCamera.h"

void ImageHandle::setup(){
    char* version = cam.getVersion();
    outputDevice.printf("Version %s\n",version);
    uint8_t targetSize=VC0706_320x240;//640x480;
    // 320x240
    //VC0706_640x480;//VC0706_160x120;
    cam.setImageSize(targetSize);
    uint8_t realSize=cam.getImageSize();
}

void ImageHandle::take(){
    if (! cam.takePicture()) {
        outputDevice.printf("Failed to snap!\n");
        while(1){}
    }
    unsigned int sID;
    
    // Send location
    sID=getPS().getSuperID();
    DH::Locs().getC().getLat()=40.006145f;
    DH::Locs().getC().getLon()=-105.262173;
    DH::Locs().getC().getAlt()=5;
    DH::Locs().getC().getHeading()=5;
    DH::Locs().getC().getTilt()=5;
    getPS().sendPacket(0,NULL,0,PT_EMPTY);
    getPS().sendPacket(sID,NULL,0,PT_IMAGEHEAD);
    getPS().sendPacket(sID,(char*)(&DH::Locs().getC().getLoc()),sizeof(DataLocation));
    getPS().sendPacket(sID,NULL,0,PT_END);
    
    // Send image
    sID=getPS().getSuperID();
    getPS().sendPacket(0,NULL,0,PT_EMPTY);
    getPS().sendPacket(sID,NULL,0,PT_IMAGE);
    int size=cam.frameLength();
    outputDevice.printf("Image Start %d %d\n",size,sizeof(PacketStruct));
    int i;
    char backBuffer[PACKETSIZE];
    int bloc=0;
    for(i=0;i<size;){
        // read 32 bytes at a time;
        uint8_t bytesToRead = std::min(64, size-i); // change 32 to 64 for a speedup but may not work with all setups!
        uint8_t bytesRead=0;
        char* buffer = (char*)cam.readPicture(bytesToRead,&bytesRead);
        for(int a=0;a<bytesRead;a++){
            if(bloc==PACKETSIZE){
                getPS().sendPacket(sID,backBuffer,bloc);
                bloc=0;
            }
            backBuffer[bloc++]=buffer[a];
        }
        //getPS().sendPacket(sID,buffer,bytesRead);
        //for(int a=0;a<bytesRead;a++){outputDevice.putc(buffer[a]);}
        i+=bytesRead;
    }
    getPS().sendPacket(sID,backBuffer,bloc);
    outputDevice.printf("Image End\n",size);
    getPS().sendPacket(sID,NULL,0,PT_END);
    cam.resumeVideo();
}

bool ImageHandle::check(){
    return true;
}

void ImageHandle::run(){
    if(!initialized){
        initialized=true;
        outputDevice.printf("Setup cam\n");
        setup();
    }
    if(check()){
        take();
        wait(2);
        //while(true){}
    }
}