Diff: ucam.cpp

Revision:

0:da6a22da11a2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ucam.cpp	Fri Dec 11 21:59:44 2009 +0000
@@ -0,0 +1,800 @@
+#include "stdafx.h"
+
+#include "mbed.h"
+#include "ucam.h"
+#include "Frame.h"
+#include "ServoMinder.h"
+#include "MotionFinder.h"
+#include "Servo.h"
+#include "SerialBuffered.h"
+#include "Blinker.h"
+
+
+// ucam protocol implementation for mbed
+
+
+Logger pcSerial(USBTX, USBRX); // tx, rx
+
+
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+
+
+
+//LocalFileSystem local("local");
+UCam ucam(p13, p14);
+
+
+
+Servo xServo (p23);
+Servo yServo (p22);
+Servo eyelidServo (p21); 
+ServoMinder *eyelidMinder = new ServoMinder( &eyelidServo );
+Blinker *blinker = new Blinker( eyelidMinder ); 
+ 
+
+MotionFinder *motionFinder = NULL;
+
+
+      
+void UCamInit() {
+   
+   
+
+    blinker->close();
+      
+    ucam.doStartup();
+    Frame::initFrames();
+    
+    blinker->open();
+  
+    motionFinder = new MotionFinder( new ServoMinder(&xServo), new ServoMinder(&yServo) );
+    
+}
+
+
+void UCamGetJpeg()
+{
+    ucam.doConfig( false, UCAM_COLOUR_JPEG, UCAM_JPEG_SIZE_640x480 ); 
+
+
+    uint8_t picType = UCAM_PIC_TYPE_JPEG_PREVIEW;
+    
+
+        
+    ucam.doGetJpegPictureToFile( picType, "C:/mbed/out.jpg" );
+    
+}
+
+Frame* UCamGetRaw( )
+
+{
+    ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60); 
+        
+    return ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW ); // returns a frame which the caller must release
+    
+}
+
+Frame* UCamGetDiff( )
+{
+    ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60 ); 
+    
+    Frame *frame = ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW );
+
+
+    motionFinder->processFrame( frame );  // returns a frame which the caller must *not* release
+
+    return motionFinder->m_resultFrame;  
+    
+}
+
+Frame* UCamResetDiff( )
+{
+    ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60 ); 
+  
+    Frame *frame = ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW );
+
+    
+    motionFinder->newBackground( frame ); 
+
+    return motionFinder->m_resultFrame; // returns a frame which the caller must *not* release
+    
+}
+
+UCam::UCam( PinName tx, PinName    rx ) : camSerial(5000, p13, p14) // tx, rx
+//UCam::UCam( PinName tx, PinName    rx ) : camSerial( p13, p14) // tx, rx
+
+{
+    lastCommand = 0;
+    m_confused = 0;
+    m_colourType = UCAM_COLOUR_NOT_SET;
+    camSerial.setTimeout( 1.0 );
+
+}
+
+void UCam::doStartup()
+{
+    pcSerial.printf("\r\n\n\nucam waiting\r\n");
+    
+   
+    wait(5); //delay to give time to get the terminal emulator up & running
+          
+    pcSerial.printf("\r\n\n\nucam running!\r\n");
+    
+    // When running on desktop over USB serial cable, this baud rate seems to need to match the rate set in the USB device configuration (via Control Panel/System/Device Manager/Properties/Advanced)
+#ifdef ON_DESKTOP
+    camSerial.baud(14400);  // lowest supported rate
+    
+#else
+    //camSerial.baud(14400);  // lowest supported rate
+    
+    //camSerial.baud(57600);   
+    
+    camSerial.baud(115200);    // highest supported rate
+#endif
+
+     myled1 = 1;
+     
+     // drain pending bytes
+     pcSerial.printf("doStartup - draining\r\n");
+     while( camSerial.readable())
+        uint8_t a = camSerial.getc();
+        
+     doConnect();
+
+    
+     
+    //wait 2 secs for camera to warm up
+    wait(2);
+
+     myled1 = 0;
+
+     pcSerial.printf("doStartup finished\r\n");
+}
+
+int UCam::doConfig( bool raw, uint8_t colourType, uint8_t imageSize )
+{   
+    myled2 = 1;
+
+    m_raw = raw;
+    m_colourType = colourType;
+    m_imageSize = imageSize;
+
+    // defaults
+    uint8_t rawSize = UCAM_RAW_SIZE_80x60;
+    uint8_t jpegSize = UCAM_JPEG_SIZE_80x64;
+
+  
+    if( m_raw )
+    {
+        switch( m_imageSize )
+        {
+            // Sizes for raw images
+        case UCAM_RAW_SIZE_80x60:
+            rawSize = m_imageSize;
+            m_width = 80;
+            m_height = 60;
+            break;
+
+        case UCAM_RAW_SIZE_160x120:
+            rawSize = m_imageSize;
+            m_width = 160;
+            m_height = 120;
+            break;
+
+        case UCAM_RAW_SIZE_320x240:
+            rawSize = m_imageSize;
+            m_width = 320;
+            m_height = 240;
+            break;
+
+        case UCAM_RAW_SIZE_640x480:
+            rawSize = m_imageSize;
+            m_width = 640;
+            m_height = 480;
+            break;
+
+        case UCAM_RAW_SIZE_128x128:
+            rawSize = m_imageSize;
+            m_width = 128;
+            m_height = 128;
+            break;
+
+        case UCAM_RAW_SIZE_128x96:
+        default:
+            rawSize = m_imageSize;
+            m_width = 128;
+            m_height = 96;
+            break;
+        }
+    }
+    else
+    {
+        // not raw - must be jpeg
+        switch( m_imageSize )
+        {
+    
+
+        case UCAM_JPEG_SIZE_80x64:
+            jpegSize = m_imageSize;
+            m_width = 80;
+            m_height = 64;
+            break;
+
+        case UCAM_JPEG_SIZE_160x128:
+            jpegSize = m_imageSize;
+            m_width = 160;
+            m_height = 128;
+            break;
+
+        case UCAM_JPEG_SIZE_320x240:
+            jpegSize = m_imageSize;
+            m_width = 320;
+            m_height = 240;
+            break;
+
+        case UCAM_JPEG_SIZE_640x480:
+        default:
+            jpegSize = m_imageSize;
+            m_width = 640;
+            m_height = 480;
+            break;
+
+        }
+
+
+        
+
+    }
+
+
+    pcSerial.printf("doConfig sending INITIAL %x %x %x\r\n", colourType, rawSize, jpegSize );
+
+    if( ! doCommand( UCAM_INITIAL, 0x00, 
+                                colourType,     // colour type - 07 for jpg
+                                rawSize,     // raw resolution
+                                jpegSize  ))  // jpeg resolution - 05 for 320x240
+        return 0;                                
+    
+    pcSerial.printf("sending package size\r\n");
+
+
+    // package size is only relevant for jpeg transfers
+    if( ! doCommand( UCAM_SET_PACKAGE_SIZE, 0x08, 
+                                        0x00,     // low byte of size
+                                        0x02,     // high byte of size
+                                        0x00 )) 
+        return 0;
+  
+
+  myled2 = 0;
+
+  return 1;
+}
+
+int UCam::fixConfusion()
+{
+    if( ! m_confused )
+        return 1;
+
+    pcSerial.printf("fixConfusion - confused\r\n");
+    for( int i = 0; i < 10; i ++ )
+    {
+        // drain pending bytes
+        pcSerial.printf("fixConfusion - draining\r\n");
+        while( camSerial.readable())
+            uint8_t a = camSerial.getc();
+        
+        // reset
+        pcSerial.printf("fixConfusion - resetting\r\n");
+
+        if( doReset())
+        {
+            wait( 0.5 );
+        // re-sync 
+            if( doSyncs())
+            {
+
+                // re-config
+
+                if(  m_colourType == UCAM_COLOUR_NOT_SET ||      // for when we are confused before we have done any config
+                     doConfig( m_raw, m_colourType, m_imageSize ))  // for when we are confused after config, so ought to restore it
+                {
+                    pcSerial.printf("fixConfusion - success\r\n");
+
+                    m_confused = 0;
+                    return 1;
+                }
+            }
+
+        }
+
+        pcSerial.printf("fixConfusion - trying again...\r\n");
+
+    }
+    
+    pcSerial.printf("fixConfusion - giving up\r\n");
+
+    m_confused = 1;
+    return 0;
+
+}
+
+Frame* UCam::doGetRawPictureToBuffer( uint8_t pictureType )
+{
+    if( ! fixConfusion())
+        return NULL;
+
+
+    if( pictureType == UCAM_PIC_TYPE_SNAPSHOT )
+        doSnapshot( UCAM_SNAPSHOT_RAW );
+
+     pcSerial.printf("sending get picture\r\n");
+     
+     myled3 = 1;
+
+    if( ! doCommand( UCAM_GET_PICTURE,   pictureType, 0x00, 0x00, 0x00 ))
+    {
+        m_confused = 1;
+        pcSerial.printf("failed GET_PICTURE - giving up\r\n");
+        return 0;
+     }
+
+
+         
+    uint32_t totalBytes = readData();
+
+    if( totalBytes < 1 )
+    {
+        m_confused = 1;
+        pcSerial.printf("totalBytes < 1 - giving up\r\n");
+        return 0;
+    }
+
+ 
+    Frame *frame = NULL;
+    Frame::allocFrame( &frame, m_colourType, m_width, m_height, totalBytes );
+
+    if( frame == NULL || frame->m_bad )
+    {
+         m_confused = 1;
+         pcSerial.printf("doGetRawPictureToBuffer - bad frame - giving up\r\n");
+        return 0;
+    }
+    
+    uint8_t *rawBuffer = frame->m_pixels;
+  
+ 
+    uint32_t actuallyRead = readBytes( rawBuffer, totalBytes );
+
+
+ 
+    sendAckForRawData();
+
+    if( actuallyRead < totalBytes )
+    {
+        m_confused = 1;
+        Frame::releaseFrame( &frame );
+        pcSerial.printf("Not enough bytes - %d  < %d \r\n", (int) actuallyRead, (int) totalBytes );
+        return NULL;
+    }
+
+    
+    pcSerial.printf("Done!\r\n");
+     
+    myled3 = 0;
+    
+    return frame;
+ }
+
+
+int UCam::doConnect()
+{
+    while( true )
+    {
+        if( doSyncs())
+        {
+            break;
+        }
+        else
+        {
+            m_confused = true;
+            if( fixConfusion())
+                break;
+        }
+    }
+
+    return 1;
+}
+
+int UCam::doSyncs()
+{
+    int i = 0;
+
+    while( true )
+    {
+        pcSerial.printf("sending sync\r\n");
+
+        wait( 0.5 );
+        
+        sendCommand( UCAM_SYNC, 0x00, 0x00, 0x00, 0x00  ); 
+        
+       //// pcSerial.printf("sent sync\r\n");
+
+        
+        if( camSerial.readable())
+        {
+            //pcSerial.printf("readable - trying ack\r\n");
+
+            if( readAckPatiently(UCAM_SYNC))
+                break;
+        }
+
+        if( i ++ > 10 )
+            return 0;
+        
+    }
+    
+    if( ! readSync())
+        return 0;
+    
+    myled1 = 1;
+
+
+    sendAck();
+    
+    return 1;
+}
+
+void UCam::sendCommand( int command, int p1, int p2, int p3, int p4 )
+{
+    camSerial.putc( (command >> 8) & 0xff );     
+    camSerial.putc( command & 0xff );    
+    camSerial.putc( p1 );
+    camSerial.putc( p2 );
+    camSerial.putc( p3 );
+    camSerial.putc( p4 );
+    
+
+}
+    
+int UCam::doCommand( int command, int p1, int p2, int p3, int p4 )
+{
+    sendCommand(  command, p1,p2, p3, p4 );
+    
+    
+    return readAck( command );
+}
+
+int UCam::doReset()
+{
+    return doCommand( UCAM_RESET, 
+                        0x00,        // 0x00 reboots camera
+                        //0x01,        // 0x01 resets state machines, does not reboot camera
+                        0x00, 0x00, 
+                        0xFF );        // 00 is a regular reset, FF causes a 'special reset' which is more immediate
+                
+                
+     
+}
+
+int UCam::doSnapshot( uint8_t snapshotType )
+{
+    return doCommand( UCAM_SNAPSHOT, snapshotType, 0x00, 0x00, 0x00 ); 
+}
+
+void UCam::sendAck()
+{
+     sendCommand( UCAM_ACK, 0x0D, 0x00, 0x00, 0x00 );
+}
+ 
+void UCam::sendAckForPackage( uint16_t p)  // requests the camera to send the data for the package with that number
+{
+     sendCommand( UCAM_ACK, 0x00, 0x00, p & 0xff,  (p >> 8) & 0xff);
+} 
+
+void UCam::sendAckForRawData( ) 
+{
+     sendCommand( UCAM_ACK, 0x00, 0x0A, 0x01, 0x00);
+} 
+
+int UCam::readAck( uint16_t command )
+{
+    
+    uint8_t bytes[6];
+    
+    readBytes( bytes, 6);
+    
+   // pcSerial.printf("ack read %x  %x %x %x %x %x \r\n", (int) bytes[0], (int) bytes[1], (int) bytes[2], (int) bytes[3], (int) bytes[4], bytes[5] );
+   
+    if( bytes[0] != 0xaa || bytes[1] != 0x0e || bytes[2] != (command & 0xff))
+    {
+        pcSerial.printf("ack read %x  %x %x %x %x %x \r\n", (int) bytes[0], (int) bytes[1], (int) bytes[2], (int) bytes[3], (int) bytes[4], bytes[5] );
+   
+        if( bytes[1] == 0x0f )
+           pcSerial.printf("ack is a NAK, error code %x for command %x\r\n", (int) bytes[4], (int) command);
+        else         
+           pcSerial.printf("ack is for wrong command! Should be for %x\r\n", (int) command);
+        m_confused = 1;
+        return 0;
+    }
+
+    return 1;
+}
+
+int UCam::readAckPatiently( uint16_t command )
+{
+    uint8_t a = 0;
+    uint16_t n = 0;
+    
+    while( n < 100 || camSerial.readable()) // used when we are waiting for a response to a sync, when we need to skip garbage bytes
+    {
+         a = camSerial.getc();
+         if( a == 0xAA )
+         {
+            // pcSerial.printf("ack read AA\r\n");
+
+             break;
+         }
+         
+         n++;
+         
+         pcSerial.printf("ackPatiently skipped %x\r\n", (int) a);
+    }
+    
+    uint8_t bytes[5];
+    
+    readBytes( bytes, 5);
+    
+   
+    if( a != 0xaa ||  bytes[1] != (command & 0xff))
+    {
+        pcSerial.printf("ackPatiently read %x  %x %x %x %x %x \r\n", (int) a, (int) bytes[0], (int) bytes[1], (int) bytes[2], (int) bytes[3], (int) bytes[4] );
+
+        pcSerial.printf("ackPatiently is for wrong command! Should be for %x\r\n", (int) command);
+        m_confused = 1;
+        return 0;
+    }
+    else
+    {
+        pcSerial.printf("ackPatiently is good!\r\n");
+    }
+
+    return 1;
+}
+
+int UCam::readSync()
+{
+    uint8_t bytes[6];
+    
+    readBytes( bytes,6 );
+    
+     // check content
+
+    
+    if( bytes[0] != 0xAA || bytes[1] != 0x0D || bytes[2] != 0x00 || bytes[3] != 0x00 || bytes[4] != 0x00 || bytes[5] != 0x00 )
+    {
+        pcSerial.printf("sync is wrong - %x %x %x %x %x %x \r\n", (int) bytes[0], (int) bytes[1], (int) bytes[2], (int) bytes[3], (int) bytes[4], (int) bytes[5] );
+        m_confused = 1;
+        return 0;
+    }
+    return 1;
+}
+
+
+
+uint16_t UCam::readUInt16()
+{
+    uint8_t bytes[2];
+    
+    readBytes( bytes, 2);
+
+    // pcSerial.printf("readUInt16 read %x %x  \r\n", (int) bytes[0], (int) bytes[1] );
+    
+   
+    uint16_t i =  bytes[1]<<8 | bytes[0];
+    
+
+    
+    return i;
+}
+
+int UCam::readBytes(uint8_t *bytes, int size )
+{
+
+    int n = camSerial.readBytes( bytes, size );
+    if( n < size )
+    {
+         m_confused = 1;
+         int m = n;
+            
+         // put some zeroes in the output to make clear it's empty
+         do
+         {
+              bytes[m] = (uint8_t) 0;
+              m ++;
+         }
+         while( m < size && m < 20 ); 
+             
+           
+    }
+    
+    return n;
+    
+      
+}
+
+
+
+
+
+int UCam::doGetJpegPictureToFile( uint8_t pictureType, char*filename )
+{
+    if( ! fixConfusion())
+        return NULL;
+
+    if( pictureType == UCAM_PIC_TYPE_SNAPSHOT )
+        doSnapshot( UCAM_SNAPSHOT_JPEG );
+
+    FILE *jpgFile = fopen(filename, FILE_WRITE_STRING); // "w" or "wb" for Windows
+    
+    if( jpgFile != NULL )
+         pcSerial.printf("opened output file\r\n");
+    
+     pcSerial.printf("sending get picture\r\n");
+     
+     myled3 = 1;
+
+    doCommand( UCAM_GET_PICTURE,   pictureType, 0x00, 0x00, 0x00 );  
+    
+    pcSerial.printf("sent get_picture\r\n");
+        
+    uint32_t totalBytes = readData();
+    
+    // pcSerial.printf("totalBytes is %d bytes\r\n", (int) totalBytes );
+    
+    uint16_t packageN = 0;
+    
+    
+    uint32_t bytesRead = 0;
+    
+    while( bytesRead < totalBytes )
+    {
+       sendAckForPackage(packageN);
+
+       int actuallyRead = readPackage( jpgFile, packageN );
+       
+       pcSerial.printf("read package of %d bytes\r\n", (int) actuallyRead );
+       
+       if( actuallyRead < 0 )
+       {
+           pcSerial.printf("didn't read enough bytes of package - giving up\r\n");
+           m_confused = 1;
+      
+           break;
+       }
+       
+       bytesRead += actuallyRead;
+        
+       packageN++;
+       
+        
+
+    }
+
+    sendAckForPackage(0xF0F0);
+    
+    fclose( jpgFile );
+    
+     pcSerial.printf("Done!\r\n");
+     
+    myled3 = 0;
+    return 1;
+ }
+    
+    
+int UCam::readPackage( FILE *jpgFile, uint16_t targetPackage )
+{
+    int actuallyRead;
+    uint16_t packageId;
+    // 2 bytes id
+    packageId = readUInt16();
+
+
+    
+   //pcSerial.printf("packageId is %d\r\n", (int)packageId );
+
+    if( packageId != targetPackage )
+    {
+         pcSerial.printf("bad package id  %d (%x)  in package header for target id %d - giving up\r\n", packageId, packageId, targetPackage);
+        
+        actuallyRead = readBytes( packageBody, 500 );
+         pcSerial.printf("next %d bytes\r\n", actuallyRead);
+        for( int i = 0 ; i < actuallyRead; i ++ )
+            pcSerial.printf("%x\r\n", packageBody[i]);
+        m_confused = 1;    
+        return -1;
+    }
+    
+    // 2 bytes data size
+    uint16_t packageSize = readUInt16();
+    
+   //pcSerial.printf("packageSize is %d bytes\r\n", (int)packageSize );
+
+
+    int dataSize =  packageSize; // - 6;
+    
+   //pcSerial.printf("dataSize is %d bytes\r\n", (int)dataSize );
+
+    if( dataSize > sizeof( packageBody )) // too big - give up
+    {
+        pcSerial.printf("bad dataSize  %d in package header for id %d - giving up\r\n", dataSize, packageId);
+        m_confused = 1;
+        return -1;
+    }
+        
+    // image data (package size - 6 bytes)
+    actuallyRead = readBytes( packageBody, dataSize);
+    
+
+    
+   //pcSerial.printf("done readBytes, read %d\r\n", actuallyRead);
+
+    if( actuallyRead < dataSize )
+    {
+        pcSerial.printf("bad readBytes, read %d\r\n", actuallyRead);
+        for( int i = 0 ; i < actuallyRead; i ++ )
+            pcSerial.printf("%x\r\n", packageBody[i]);
+        m_confused = 1;
+        return -1;
+    }
+        
+    // 2 bytes verify code
+    uint16_t verifyCode = readUInt16();
+    
+    pcSerial.printf("done readBytes for package, read %d\r\n", actuallyRead);
+    pcSerial.printf("verifyCode %d\r\n", verifyCode);
+    
+    fwrite( packageBody, 1, actuallyRead, jpgFile );
+    
+    pcSerial.printf("done fwrite\r\n" );
+
+    
+    return actuallyRead;
+
+}    
+
+uint32_t UCam::readData()
+{
+    uint8_t bytes[6];
+    
+    if( 6 != readBytes( bytes, 6))
+    {
+        pcSerial.printf("readData failed to read 6 bytes\r\n");
+        m_confused = 1;
+        return 0;
+    }
+
+    uint32_t totalSize = ( bytes[5]<<16 ) | ( bytes[4]<<8 ) | ( bytes[3] );
+
+    // check content - AA 0A tt nn nn nn - tt is the image type, nn tells us the image size
+
+    // only log fail cases here, otherwise the logging slows us down
+    // and we miss the image data, which is coming along already
+    
+    if( bytes[0] != 0xAA || bytes[1] != 0x0A)
+    {
+        pcSerial.printf("readData totalSize %d - read %x %x %x %x %x %x \r\n", (int) totalSize, (int) bytes[0], (int) bytes[1], (int) bytes[2], (int) bytes[3], (int) bytes[4], (int) bytes[5] );
+        
+         pcSerial.printf("readData failed\r\n");
+         m_confused = 1;
+         return 0;
+    }
+        
+    return totalSize;
+}
+