![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is the code we showed at Uncraftivism
ucam.cpp
- Committer:
- jarkman
- Date:
- 2009-11-20
- Revision:
- 0:57f4fdadc97f
- Child:
- 1:70d90598d2e7
File content as of revision 0:57f4fdadc97f:
#include "stdafx.h" #include "mbed.h" #include "ucam.h" #include "Frame.h" #include "ServoMinder.h" #include "MotionFinder.h" #include "Servo.h" #include "SerialBuffered.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 (p21); Servo yServo (p22); MotionFinder *motionFinder = NULL; void UCamInit() { ucam.doStartup(); Frame::initFrames(); 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; }