ucam.cpp

00001 #include "ucam.h"
00002 /*commands*/
00003 #define CMD_ACK         0x0E
00004 #define CMD_SYNC        0x0D
00005 #define CMD_INITIAL     0x01
00006 #define CMD_SNAPSHOT    0x05
00007 #define CMD_PICTURE     0x04
00008 #define CMD_DATA        0x0A
00009 #define CMD_BAUDRATE    0x07
00010 #define MAX_SYNC        60
00011 #define PACKET_LENGTH   506
00012 #define BUFFER_SIZE     8129
00013 #define DEBUG_WARN      0x01
00014 #define DEBUG_INFO      0x02
00015 #define DEBUG_COMS      0x04
00016 #define DEBUG_LEVEL     0xff
00017 
00018 #if DEBUG_LEVEL
00019     extern Serial debug;
00020 #endif
00021 
00022 struct ucam_command{
00023     union{
00024         struct{
00025             uint8_t buf[6];
00026         };
00027         struct{
00028             uint8_t pre;
00029             uint8_t id;            
00030             uint8_t p1;
00031             uint8_t p2;
00032             uint8_t p3;
00033             uint8_t p4;
00034         };
00035     };
00036 };
00037 
00038 struct{
00039     uint16_t ss; 
00040     uint16_t se;
00041     uint8_t sz[BUFFER_SIZE];
00042 }fbuf;
00043   
00044 uint8_t pbuf[PACKET_LENGTH * 2];
00045 
00046 inline void buf_put_char(uint8_t ch)
00047 {  
00048    register uint16_t se = fbuf.se;
00049    register uint16_t idx = (se + 1) % BUFFER_SIZE;
00050     
00051    if (idx != fbuf.ss) {
00052        fbuf.sz[se] = ch;
00053        fbuf.se = idx;    
00054    }
00055    
00056    ///*buffer is full*/
00057    //#if (DEBUG_LEVEL & DEBUG_WARN)
00058    //   debug.printf("warning buffer full\n");
00059    //#endif
00060 }
00061 
00062 uint8_t buf_get_char()
00063 {
00064    uint16_t ch = fbuf.ss % BUFFER_SIZE;
00065    
00066    while( ch == fbuf.se){
00067       /*buffer is empty*/
00068         wait_ms(1);
00069    }
00070       
00071    ch = fbuf.sz[fbuf.ss];
00072    fbuf.ss = (fbuf.ss + 1) % BUFFER_SIZE;
00073    return ch;
00074 }
00075 
00076 UCam::UCam(PinName tx_pin, PinName rx_pin, PinName led_pin, uint32_t baudrate):
00077 led(led_pin)
00078 {
00079     ucam = new Serial(p13, p14);
00080     ucam->baud(115200);
00081     ucam->attach(this, &UCam::uart_isr);
00082     fbuf.ss = 0;
00083     fbuf.se = 0;
00084 }
00085 
00086 uint16_t buf_size()
00087 {
00088     return fbuf.se - fbuf.ss;
00089 }
00090 
00091 UCam::~UCam()
00092 {
00093     led = 0;
00094     delete ucam;
00095 }
00096 
00097 void UCam::uart_isr(void)
00098 {
00099     register uint16_t i;
00100     register uint16_t size = 0;
00101     register uint8_t *buf = pbuf;
00102            
00103     do{
00104         buf[size++] = ucam->getc();
00105     }while (ucam->readable());        
00106     
00107     for (i=0; i<size; i++)  
00108         buf_put_char(buf[i]);
00109 }
00110 
00111 int8_t UCam::connect()
00112 {      
00113     struct ucam_command cmd;
00114     
00115     /*reset state machine*/
00116     //send_cmd(0x08, 0x01, 0x00, 0x00, 0x00);
00117     //wait(1.0);
00118 
00119     /*full reset*/    
00120  //   send_cmd(0x08, 0x00, 0x00, 0x00, 0xff);
00121    // wait(1.0);
00122 
00123     int8_t state=0;
00124 #if (DEBUG_LEVEL & DEBUG_INFO)
00125         debug.puts("syncing...\n");
00126 #endif
00127             
00128     for (int i = 0; i < MAX_SYNC; i++) {
00129         switch(state){
00130             case 0: /*sync and recv ACK*/
00131                 if (send_cmd(CMD_SYNC) == 0)
00132                     ++state;
00133                 break;
00134             
00135             case 1:/*recv SYNC*/
00136                 if (recv_cmd(&cmd) == 0 && cmd.id == CMD_SYNC)
00137                     ++state;
00138                 break;                
00139             
00140             case 2:/*send ACK SYNC*/
00141                 if (send_cmd(CMD_ACK, CMD_SYNC, 0x00, 0x00, 0x00, 0x01) == 0)
00142                     ++state; 
00143                 break;
00144                 
00145             case 3:
00146                 led = 1; 
00147                 send_cmd(CMD_BAUDRATE, 0x00, 0x04, 0x00, 0x00);
00148                 ucam->baud(737280);
00149                 return 0;
00150         }        
00151         wait(0.100);              
00152     }
00153     
00154     return -1;
00155 }
00156 
00157 int8_t UCam::send_cmd(uint8_t id, uint8_t p1, uint8_t p2, uint8_t p3, uint8_t p4, uint8_t noack)
00158 {
00159     struct ucam_command cmd;
00160 
00161     ucam->putc(0xAA);
00162     ucam->putc(id);
00163     ucam->putc(p1);
00164     ucam->putc(p2);
00165     ucam->putc(p3);
00166     ucam->putc(p4);           
00167 
00168 #if (DEBUG_LEVEL & DEBUG_COMS)
00169     debug.printf("> aa %02x %02x %02x %02x %02x\n", 
00170                     id, p1, p2, p3, p4);
00171 #endif/*enable_debug*/
00172                      
00173     if (noack)
00174         return 0;
00175     
00176     /*recv ACK*/    
00177     if (recv_cmd(&cmd) == 0 && cmd.id == CMD_ACK && cmd.p1 == id)
00178         return 0;
00179 
00180     return -1;
00181 }
00182 
00183 int8_t UCam::recv_cmd(struct ucam_command *cmd)
00184 { 
00185     for (int8_t i = 0; i < sizeof(struct ucam_command); i++) {            
00186         cmd->buf[i] = buf_get_char();
00187     }    
00188 
00189 #if (DEBUG_LEVEL & DEBUG_COMS)
00190     debug.printf("< %02x %02x %02x %02x %02x %02x\n", 
00191                     cmd->pre, cmd->id, cmd->p1,
00192                     cmd->p2, cmd->p3, cmd->p4);
00193 #endif/*enable_debug*/
00194  
00195     if (cmd->pre != 0xAA)
00196         goto error;          
00197 
00198     return 0; 
00199 error:
00200     return -1;   
00201 }
00202 
00203 int8_t UCam::snapshot(uint8_t *buf, uint16_t *len, uint8_t color_space, uint8_t raw_size, uint8_t jpeg_size)
00204 { 
00205     struct ucam_command cmd;
00206     
00207     if (send_cmd(CMD_INITIAL, 0x00, color_space, raw_size, jpeg_size) != 0)
00208         return -1;
00209 
00210     if (send_cmd(0x06, 0x08, 0x00, 0x02, 0x00) != 0) //set packet size to 512
00211         return -2;       
00212      
00213     if (send_cmd(CMD_SNAPSHOT, 0x00, 0x00, 0x00, 0x00) != 0)
00214         return -3;
00215        
00216     if (send_cmd(CMD_PICTURE, 0x01, 0x00, 0x00, 0x00) != 0)
00217         return -4;
00218 
00219     if (recv_cmd(&cmd) != 0 || cmd.id != CMD_DATA)
00220         return -5;            
00221           
00222     uint8_t id = 0;
00223     uint8_t byte;
00224     uint8_t blocks;
00225 
00226     uint32_t size = 0, i = 0;    
00227     
00228     size = (size | cmd.p4) << 16;
00229     size = (size | cmd.p3) << 8;    
00230     size = (size | cmd.p2);
00231     
00232     blocks = size / PACKET_LENGTH;
00233     if (size % PACKET_LENGTH)
00234         ++blocks;
00235         
00236 #if (DEBUG_LEVEL & DEBUG_INFO)
00237     debug.printf("image %d %d\n", size, blocks);
00238 #endif/*enable_debug*/
00239     
00240     *len = size;
00241     while (blocks--) {
00242         if (send_cmd(CMD_ACK, 0x00, 0x00, id++, 0x00, true) != 0)
00243             return -6;
00244 
00245         buf_get_char();  //id lsb
00246         buf_get_char();  //id msb
00247         
00248         size=0;
00249         byte = buf_get_char(); //size lsb
00250         size |= buf_get_char(); //size msb
00251         size = (size << 8) | byte;
00252         
00253         if (size == 0 || size > PACKET_LENGTH)
00254             return -7;
00255                 
00256         for (i = 0; i<size; i++) {
00257             *buf++ = buf_get_char(); 
00258         }
00259         buf_get_char(); //checksum lsb
00260         buf_get_char(); //checksum msb                      
00261     }        
00262             
00263     /*ack frame*/
00264     send_cmd(CMD_ACK, 0x00, 0x00, 0xF0, 0xF0, 0x01);
00265     return 0;
00266 }
00267 
00268 int8_t UCam::start_video(uint8_t color_space, uint8_t raw_size, uint8_t jpeg_size)
00269 {   
00270     if (send_cmd(CMD_INITIAL, 0x00, color_space, raw_size, jpeg_size) != 0)
00271         return -1;
00272 
00273     if (send_cmd(0x06, 0x08, 0x00, 0x02, 0x00) != 0) //set packet size to 512
00274         return -2;       
00275          
00276     return 0;
00277 }
00278 
00279 int8_t UCam::next_frame(uint8_t *buf, uint16_t *len)
00280 {   
00281     uint8_t id = 0;
00282     uint8_t byte;
00283     uint8_t blocks;
00284     uint32_t size = 0, i = 0;    
00285     
00286     struct ucam_command cmd;
00287     
00288     wait_ms(10); /* don't know why*/
00289     
00290     if (send_cmd(CMD_PICTURE, 0x05, 0x00, 0x00, 0x00) != 0)
00291         goto error;
00292                
00293     if (recv_cmd(&cmd) != 0 || cmd.id != CMD_DATA)
00294         goto error;           
00295           
00296     
00297     size = (size | cmd.p4) << 16;
00298     size = (size | cmd.p3) << 8;    
00299     size = (size | cmd.p2);
00300     
00301     blocks = size / PACKET_LENGTH;
00302     if (size % PACKET_LENGTH)
00303         ++blocks;
00304         
00305     #if (DEBUG_LEVEL & DEBUG_WARN)
00306         debug.printf("image %d %d\n", size, blocks);
00307     #endif/*enable_debug*/
00308     
00309     *len = size;
00310     while (blocks--) {
00311         if (send_cmd(CMD_ACK, 0x00, 0x00, id++, 0x00, true) != 0)
00312             goto error;
00313         
00314         size = 0;
00315         byte = buf_get_char(); //id lsb
00316         size |= buf_get_char(); //id msb
00317         size = (size << 8) | byte;               
00318             
00319         #if (DEBUG_LEVEL & DEBUG_INFO)
00320             debug.printf("block id %d ", size);
00321         #endif
00322         
00323         size=0;
00324         byte = buf_get_char(); //size lsb
00325         size |= buf_get_char(); //size msb
00326         size = (size << 8) | byte;
00327 
00328         #if (DEBUG_LEVEL & DEBUG_INFO)
00329             debug.printf("block size %d\n", size);
00330         #endif
00331         
00332         if (size > PACKET_LENGTH || size > buf_size()) {
00333             #if (DEBUG_LEVEL & DEBUG_INFO)
00334                 debug.printf("warning: invalid size %d\n", size);
00335             #endif
00336             goto error;
00337         }
00338                 
00339         if (blocks == 0 && size == PACKET_LENGTH) { /*ucam bug*/
00340             #if (DEBUG_LEVEL & DEBUG_WARN)
00341                 debug.printf("warning: soft reset\n");
00342             #endif
00343             /*soft reset*/
00344             wait(1.0);
00345             send_cmd(0x08, 0x01, 0x00, 0x00, 0xFF);
00346             
00347             /*ack frame*/
00348             send_cmd(CMD_ACK, 0x00, 0x00, 0xF0, 0xF0, 0x01);
00349             goto error;
00350         }
00351                                       
00352         for (i = 0; i<size; i++) {
00353             *buf++ = buf_get_char();  
00354         }
00355         
00356         buf_get_char(); //checksum lsb
00357         buf_get_char(); //checksum msb                      
00358     }        
00359             
00360     /*ack frame*/
00361     send_cmd(CMD_ACK, 0x00, 0x00, 0xF0, 0xF0, 0x01);    
00362     return 0;
00363 error:
00364     return -1;
00365 }