ucam.cpp

00001 #include "stdafx.h"
00002 
00003 #include "mbed.h"
00004 #include "ucam.h"
00005 #include "Frame.h"
00006 #include "ServoMinder.h"
00007 #include "MotionFinder.h"
00008 #include "Servo.h"
00009 #include "SerialBuffered.h"
00010 #include "Blinker.h"
00011 
00012 
00013 // ucam protocol implementation for mbed
00014 
00015 
00016 Logger pcSerial(USBTX, USBRX); // tx, rx
00017 
00018 
00019 
00020 DigitalOut myled1(LED1);
00021 DigitalOut myled2(LED2);
00022 DigitalOut myled3(LED3);
00023 DigitalOut myled4(LED4);
00024 
00025 
00026 
00027 
00028 //LocalFileSystem local("local");
00029 UCam ucam(p13, p14);
00030 
00031 
00032 
00033 Servo xServo (p23);
00034 Servo yServo (p22);
00035 Servo eyelidServo (p21); 
00036 ServoMinder *eyelidMinder = new ServoMinder( &eyelidServo );
00037 Blinker *blinker = new Blinker( eyelidMinder ); 
00038  
00039 
00040 MotionFinder *motionFinder = NULL;
00041 
00042 
00043       
00044 void UCamInit() {
00045    
00046    
00047 
00048     blinker->close();
00049       
00050     ucam.doStartup();
00051     Frame::initFrames();
00052     
00053     blinker->open();
00054   
00055     motionFinder = new MotionFinder( new ServoMinder(&xServo), new ServoMinder(&yServo) );
00056     
00057 }
00058 
00059 
00060 void UCamGetJpeg()
00061 {
00062     ucam.doConfig( false, UCAM_COLOUR_JPEG, UCAM_JPEG_SIZE_640x480 ); 
00063 
00064 
00065     uint8_t picType = UCAM_PIC_TYPE_JPEG_PREVIEW;
00066     
00067 
00068         
00069     ucam.doGetJpegPictureToFile( picType, "C:/mbed/out.jpg" );
00070     
00071 }
00072 
00073 Frame* UCamGetRaw( )
00074 
00075 {
00076     ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60); 
00077         
00078     return ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW ); // returns a frame which the caller must release
00079     
00080 }
00081 
00082 Frame* UCamGetDiff( )
00083 {
00084     ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60 ); 
00085     
00086     Frame *frame = ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW );
00087 
00088 
00089     motionFinder->processFrame( frame );  // returns a frame which the caller must *not* release
00090 
00091     return motionFinder->m_resultFrame;  
00092     
00093 }
00094 
00095 Frame* UCamResetDiff( )
00096 {
00097     ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60 ); 
00098   
00099     Frame *frame = ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW );
00100 
00101     
00102     motionFinder->newBackground( frame ); 
00103 
00104     return motionFinder->m_resultFrame; // returns a frame which the caller must *not* release
00105     
00106 }
00107 
00108 UCam::UCam( PinName tx, PinName    rx ) : camSerial(5000, p13, p14) // tx, rx
00109 //UCam::UCam( PinName tx, PinName    rx ) : camSerial( p13, p14) // tx, rx
00110 
00111 {
00112     lastCommand = 0;
00113     m_confused = 0;
00114     m_colourType = UCAM_COLOUR_NOT_SET;
00115     camSerial.setTimeout( 1.0 );
00116 
00117 }
00118 
00119 void UCam::doStartup()
00120 {
00121     pcSerial.printf("\r\n\n\nucam waiting\r\n");
00122     
00123    
00124     wait(5); //delay to give time to get the terminal emulator up & running
00125           
00126     pcSerial.printf("\r\n\n\nucam running!\r\n");
00127     
00128     // 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)
00129 #ifdef ON_DESKTOP
00130     camSerial.baud(14400);  // lowest supported rate
00131     
00132 #else
00133     //camSerial.baud(14400);  // lowest supported rate
00134     
00135     //camSerial.baud(57600);   
00136     
00137     camSerial.baud(115200);    // highest supported rate
00138 #endif
00139 
00140      myled1 = 1;
00141      
00142      // drain pending bytes
00143      pcSerial.printf("doStartup - draining\r\n");
00144      while( camSerial.readable())
00145         uint8_t a = camSerial.getc();
00146         
00147      doConnect();
00148 
00149     
00150      
00151     //wait 2 secs for camera to warm up
00152     wait(2);
00153 
00154      myled1 = 0;
00155 
00156      pcSerial.printf("doStartup finished\r\n");
00157 }
00158 
00159 int UCam::doConfig( bool raw, uint8_t colourType, uint8_t imageSize )
00160 {   
00161     myled2 = 1;
00162 
00163     m_raw = raw;
00164     m_colourType = colourType;
00165     m_imageSize = imageSize;
00166 
00167     // defaults
00168     uint8_t rawSize = UCAM_RAW_SIZE_80x60;
00169     uint8_t jpegSize = UCAM_JPEG_SIZE_80x64;
00170 
00171   
00172     if( m_raw )
00173     {
00174         switch( m_imageSize )
00175         {
00176             // Sizes for raw images
00177         case UCAM_RAW_SIZE_80x60:
00178             rawSize = m_imageSize;
00179             m_width = 80;
00180             m_height = 60;
00181             break;
00182 
00183         case UCAM_RAW_SIZE_160x120:
00184             rawSize = m_imageSize;
00185             m_width = 160;
00186             m_height = 120;
00187             break;
00188 
00189         case UCAM_RAW_SIZE_320x240:
00190             rawSize = m_imageSize;
00191             m_width = 320;
00192             m_height = 240;
00193             break;
00194 
00195         case UCAM_RAW_SIZE_640x480:
00196             rawSize = m_imageSize;
00197             m_width = 640;
00198             m_height = 480;
00199             break;
00200 
00201         case UCAM_RAW_SIZE_128x128:
00202             rawSize = m_imageSize;
00203             m_width = 128;
00204             m_height = 128;
00205             break;
00206 
00207         case UCAM_RAW_SIZE_128x96:
00208         default:
00209             rawSize = m_imageSize;
00210             m_width = 128;
00211             m_height = 96;
00212             break;
00213         }
00214     }
00215     else
00216     {
00217         // not raw - must be jpeg
00218         switch( m_imageSize )
00219         {
00220     
00221 
00222         case UCAM_JPEG_SIZE_80x64:
00223             jpegSize = m_imageSize;
00224             m_width = 80;
00225             m_height = 64;
00226             break;
00227 
00228         case UCAM_JPEG_SIZE_160x128:
00229             jpegSize = m_imageSize;
00230             m_width = 160;
00231             m_height = 128;
00232             break;
00233 
00234         case UCAM_JPEG_SIZE_320x240:
00235             jpegSize = m_imageSize;
00236             m_width = 320;
00237             m_height = 240;
00238             break;
00239 
00240         case UCAM_JPEG_SIZE_640x480:
00241         default:
00242             jpegSize = m_imageSize;
00243             m_width = 640;
00244             m_height = 480;
00245             break;
00246 
00247         }
00248 
00249 
00250         
00251 
00252     }
00253 
00254 
00255     pcSerial.printf("doConfig sending INITIAL %x %x %x\r\n", colourType, rawSize, jpegSize );
00256 
00257     if( ! doCommand( UCAM_INITIAL, 0x00, 
00258                                 colourType,     // colour type - 07 for jpg
00259                                 rawSize,     // raw resolution
00260                                 jpegSize  ))  // jpeg resolution - 05 for 320x240
00261         return 0;                                
00262     
00263     pcSerial.printf("sending package size\r\n");
00264 
00265 
00266     // package size is only relevant for jpeg transfers
00267     if( ! doCommand( UCAM_SET_PACKAGE_SIZE, 0x08, 
00268                                         0x00,     // low byte of size
00269                                         0x02,     // high byte of size
00270                                         0x00 )) 
00271         return 0;
00272   
00273 
00274   myled2 = 0;
00275 
00276   return 1;
00277 }
00278 
00279 int UCam::fixConfusion()
00280 {
00281     if( ! m_confused )
00282         return 1;
00283 
00284     pcSerial.printf("fixConfusion - confused\r\n");
00285     for( int i = 0; i < 10; i ++ )
00286     {
00287         // drain pending bytes
00288         pcSerial.printf("fixConfusion - draining\r\n");
00289         while( camSerial.readable())
00290             uint8_t a = camSerial.getc();
00291         
00292         // reset
00293         pcSerial.printf("fixConfusion - resetting\r\n");
00294 
00295         if( doReset())
00296         {
00297             wait( 0.5 );
00298         // re-sync 
00299             if( doSyncs())
00300             {
00301 
00302                 // re-config
00303 
00304                 if(  m_colourType == UCAM_COLOUR_NOT_SET ||      // for when we are confused before we have done any config
00305                      doConfig( m_raw, m_colourType, m_imageSize ))  // for when we are confused after config, so ought to restore it
00306                 {
00307                     pcSerial.printf("fixConfusion - success\r\n");
00308 
00309                     m_confused = 0;
00310                     return 1;
00311                 }
00312             }
00313 
00314         }
00315 
00316         pcSerial.printf("fixConfusion - trying again...\r\n");
00317 
00318     }
00319     
00320     pcSerial.printf("fixConfusion - giving up\r\n");
00321 
00322     m_confused = 1;
00323     return 0;
00324 
00325 }
00326 
00327 Frame* UCam::doGetRawPictureToBuffer( uint8_t pictureType )
00328 {
00329     if( ! fixConfusion())
00330         return NULL;
00331 
00332 
00333     if( pictureType == UCAM_PIC_TYPE_SNAPSHOT )
00334         doSnapshot( UCAM_SNAPSHOT_RAW );
00335 
00336      pcSerial.printf("sending get picture\r\n");
00337      
00338      myled3 = 1;
00339 
00340     if( ! doCommand( UCAM_GET_PICTURE,   pictureType, 0x00, 0x00, 0x00 ))
00341     {
00342         m_confused = 1;
00343         pcSerial.printf("failed GET_PICTURE - giving up\r\n");
00344         return 0;
00345      }
00346 
00347 
00348          
00349     uint32_t totalBytes = readData();
00350 
00351     if( totalBytes < 1 )
00352     {
00353         m_confused = 1;
00354         pcSerial.printf("totalBytes < 1 - giving up\r\n");
00355         return 0;
00356     }
00357 
00358  
00359     Frame *frame = NULL;
00360     Frame::allocFrame( &frame, m_colourType, m_width, m_height, totalBytes );
00361 
00362     if( frame == NULL || frame->m_bad )
00363     {
00364          m_confused = 1;
00365          pcSerial.printf("doGetRawPictureToBuffer - bad frame - giving up\r\n");
00366         return 0;
00367     }
00368     
00369     uint8_t *rawBuffer = frame->m_pixels;
00370   
00371  
00372     uint32_t actuallyRead = readBytes( rawBuffer, totalBytes );
00373 
00374 
00375  
00376     sendAckForRawData();
00377 
00378     if( actuallyRead < totalBytes )
00379     {
00380         m_confused = 1;
00381         Frame::releaseFrame( &frame );
00382         pcSerial.printf("Not enough bytes - %d  < %d \r\n", (int) actuallyRead, (int) totalBytes );
00383         return NULL;
00384     }
00385 
00386     
00387     pcSerial.printf("Done!\r\n");
00388      
00389     myled3 = 0;
00390     
00391     return frame;
00392  }
00393 
00394 
00395 int UCam::doConnect()
00396 {
00397     while( true )
00398     {
00399         if( doSyncs())
00400         {
00401             break;
00402         }
00403         else
00404         {
00405             m_confused = true;
00406             if( fixConfusion())
00407                 break;
00408         }
00409     }
00410 
00411     return 1;
00412 }
00413 
00414 int UCam::doSyncs()
00415 {
00416     int i = 0;
00417 
00418     while( true )
00419     {
00420         pcSerial.printf("sending sync\r\n");
00421 
00422         wait( 0.5 );
00423         
00424         sendCommand( UCAM_SYNC, 0x00, 0x00, 0x00, 0x00  ); 
00425         
00426        //// pcSerial.printf("sent sync\r\n");
00427 
00428         
00429         if( camSerial.readable())
00430         {
00431             //pcSerial.printf("readable - trying ack\r\n");
00432 
00433             if( readAckPatiently(UCAM_SYNC))
00434                 break;
00435         }
00436 
00437         if( i ++ > 10 )
00438             return 0;
00439         
00440     }
00441     
00442     if( ! readSync())
00443         return 0;
00444     
00445     myled1 = 1;
00446 
00447 
00448     sendAck();
00449     
00450     return 1;
00451 }
00452 
00453 void UCam::sendCommand( int command, int p1, int p2, int p3, int p4 )
00454 {
00455     camSerial.putc( (command >> 8) & 0xff );     
00456     camSerial.putc( command & 0xff );    
00457     camSerial.putc( p1 );
00458     camSerial.putc( p2 );
00459     camSerial.putc( p3 );
00460     camSerial.putc( p4 );
00461     
00462 
00463 }
00464     
00465 int UCam::doCommand( int command, int p1, int p2, int p3, int p4 )
00466 {
00467     sendCommand(  command, p1,p2, p3, p4 );
00468     
00469     
00470     return readAck( command );
00471 }
00472 
00473 int UCam::doReset()
00474 {
00475     return doCommand( UCAM_RESET, 
00476                         0x00,        // 0x00 reboots camera
00477                         //0x01,        // 0x01 resets state machines, does not reboot camera
00478                         0x00, 0x00, 
00479                         0xFF );        // 00 is a regular reset, FF causes a 'special reset' which is more immediate
00480                 
00481                 
00482      
00483 }
00484 
00485 int UCam::doSnapshot( uint8_t snapshotType )
00486 {
00487     return doCommand( UCAM_SNAPSHOT, snapshotType, 0x00, 0x00, 0x00 ); 
00488 }
00489 
00490 void UCam::sendAck()
00491 {
00492      sendCommand( UCAM_ACK, 0x0D, 0x00, 0x00, 0x00 );
00493 }
00494  
00495 void UCam::sendAckForPackage( uint16_t p)  // requests the camera to send the data for the package with that number
00496 {
00497      sendCommand( UCAM_ACK, 0x00, 0x00, p & 0xff,  (p >> 8) & 0xff);
00498 } 
00499 
00500 void UCam::sendAckForRawData( ) 
00501 {
00502      sendCommand( UCAM_ACK, 0x00, 0x0A, 0x01, 0x00);
00503 } 
00504 
00505 int UCam::readAck( uint16_t command )
00506 {
00507     
00508     uint8_t bytes[6];
00509     
00510     readBytes( bytes, 6);
00511     
00512    // 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] );
00513    
00514     if( bytes[0] != 0xaa || bytes[1] != 0x0e || bytes[2] != (command & 0xff))
00515     {
00516         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] );
00517    
00518         if( bytes[1] == 0x0f )
00519            pcSerial.printf("ack is a NAK, error code %x for command %x\r\n", (int) bytes[4], (int) command);
00520         else         
00521            pcSerial.printf("ack is for wrong command! Should be for %x\r\n", (int) command);
00522         m_confused = 1;
00523         return 0;
00524     }
00525 
00526     return 1;
00527 }
00528 
00529 int UCam::readAckPatiently( uint16_t command )
00530 {
00531     uint8_t a = 0;
00532     uint16_t n = 0;
00533     
00534     while( n < 100 || camSerial.readable()) // used when we are waiting for a response to a sync, when we need to skip garbage bytes
00535     {
00536          a = camSerial.getc();
00537          if( a == 0xAA )
00538          {
00539             // pcSerial.printf("ack read AA\r\n");
00540 
00541              break;
00542          }
00543          
00544          n++;
00545          
00546          pcSerial.printf("ackPatiently skipped %x\r\n", (int) a);
00547     }
00548     
00549     uint8_t bytes[5];
00550     
00551     readBytes( bytes, 5);
00552     
00553    
00554     if( a != 0xaa ||  bytes[1] != (command & 0xff))
00555     {
00556         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] );
00557 
00558         pcSerial.printf("ackPatiently is for wrong command! Should be for %x\r\n", (int) command);
00559         m_confused = 1;
00560         return 0;
00561     }
00562     else
00563     {
00564         pcSerial.printf("ackPatiently is good!\r\n");
00565     }
00566 
00567     return 1;
00568 }
00569 
00570 int UCam::readSync()
00571 {
00572     uint8_t bytes[6];
00573     
00574     readBytes( bytes,6 );
00575     
00576      // check content
00577 
00578     
00579     if( bytes[0] != 0xAA || bytes[1] != 0x0D || bytes[2] != 0x00 || bytes[3] != 0x00 || bytes[4] != 0x00 || bytes[5] != 0x00 )
00580     {
00581         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] );
00582         m_confused = 1;
00583         return 0;
00584     }
00585     return 1;
00586 }
00587 
00588 
00589 
00590 uint16_t UCam::readUInt16()
00591 {
00592     uint8_t bytes[2];
00593     
00594     readBytes( bytes, 2);
00595 
00596     // pcSerial.printf("readUInt16 read %x %x  \r\n", (int) bytes[0], (int) bytes[1] );
00597     
00598    
00599     uint16_t i =  bytes[1]<<8 | bytes[0];
00600     
00601 
00602     
00603     return i;
00604 }
00605 
00606 int UCam::readBytes(uint8_t *bytes, int size )
00607 {
00608 
00609     int n = camSerial.readBytes( bytes, size );
00610     if( n < size )
00611     {
00612          m_confused = 1;
00613          int m = n;
00614             
00615          // put some zeroes in the output to make clear it's empty
00616          do
00617          {
00618               bytes[m] = (uint8_t) 0;
00619               m ++;
00620          }
00621          while( m < size && m < 20 ); 
00622              
00623            
00624     }
00625     
00626     return n;
00627     
00628       
00629 }
00630 
00631 
00632 
00633 
00634 
00635 int UCam::doGetJpegPictureToFile( uint8_t pictureType, char*filename )
00636 {
00637     if( ! fixConfusion())
00638         return NULL;
00639 
00640     if( pictureType == UCAM_PIC_TYPE_SNAPSHOT )
00641         doSnapshot( UCAM_SNAPSHOT_JPEG );
00642 
00643     FILE *jpgFile = fopen(filename, FILE_WRITE_STRING); // "w" or "wb" for Windows
00644     
00645     if( jpgFile != NULL )
00646          pcSerial.printf("opened output file\r\n");
00647     
00648      pcSerial.printf("sending get picture\r\n");
00649      
00650      myled3 = 1;
00651 
00652     doCommand( UCAM_GET_PICTURE,   pictureType, 0x00, 0x00, 0x00 );  
00653     
00654     pcSerial.printf("sent get_picture\r\n");
00655         
00656     uint32_t totalBytes = readData();
00657     
00658     // pcSerial.printf("totalBytes is %d bytes\r\n", (int) totalBytes );
00659     
00660     uint16_t packageN = 0;
00661     
00662     
00663     uint32_t bytesRead = 0;
00664     
00665     while( bytesRead < totalBytes )
00666     {
00667        sendAckForPackage(packageN);
00668 
00669        int actuallyRead = readPackage( jpgFile, packageN );
00670        
00671        pcSerial.printf("read package of %d bytes\r\n", (int) actuallyRead );
00672        
00673        if( actuallyRead < 0 )
00674        {
00675            pcSerial.printf("didn't read enough bytes of package - giving up\r\n");
00676            m_confused = 1;
00677       
00678            break;
00679        }
00680        
00681        bytesRead += actuallyRead;
00682         
00683        packageN++;
00684        
00685         
00686 
00687     }
00688 
00689     sendAckForPackage(0xF0F0);
00690     
00691     fclose( jpgFile );
00692     
00693      pcSerial.printf("Done!\r\n");
00694      
00695     myled3 = 0;
00696     return 1;
00697  }
00698     
00699     
00700 int UCam::readPackage( FILE *jpgFile, uint16_t targetPackage )
00701 {
00702     int actuallyRead;
00703     uint16_t packageId;
00704     // 2 bytes id
00705     packageId = readUInt16();
00706 
00707 
00708     
00709    //pcSerial.printf("packageId is %d\r\n", (int)packageId );
00710 
00711     if( packageId != targetPackage )
00712     {
00713          pcSerial.printf("bad package id  %d (%x)  in package header for target id %d - giving up\r\n", packageId, packageId, targetPackage);
00714         
00715         actuallyRead = readBytes( packageBody, 500 );
00716          pcSerial.printf("next %d bytes\r\n", actuallyRead);
00717         for( int i = 0 ; i < actuallyRead; i ++ )
00718             pcSerial.printf("%x\r\n", packageBody[i]);
00719         m_confused = 1;    
00720         return -1;
00721     }
00722     
00723     // 2 bytes data size
00724     uint16_t packageSize = readUInt16();
00725     
00726    //pcSerial.printf("packageSize is %d bytes\r\n", (int)packageSize );
00727 
00728 
00729     int dataSize =  packageSize; // - 6;
00730     
00731    //pcSerial.printf("dataSize is %d bytes\r\n", (int)dataSize );
00732 
00733     if( dataSize > sizeof( packageBody )) // too big - give up
00734     {
00735         pcSerial.printf("bad dataSize  %d in package header for id %d - giving up\r\n", dataSize, packageId);
00736         m_confused = 1;
00737         return -1;
00738     }
00739         
00740     // image data (package size - 6 bytes)
00741     actuallyRead = readBytes( packageBody, dataSize);
00742     
00743 
00744     
00745    //pcSerial.printf("done readBytes, read %d\r\n", actuallyRead);
00746 
00747     if( actuallyRead < dataSize )
00748     {
00749         pcSerial.printf("bad readBytes, read %d\r\n", actuallyRead);
00750         for( int i = 0 ; i < actuallyRead; i ++ )
00751             pcSerial.printf("%x\r\n", packageBody[i]);
00752         m_confused = 1;
00753         return -1;
00754     }
00755         
00756     // 2 bytes verify code
00757     uint16_t verifyCode = readUInt16();
00758     
00759     pcSerial.printf("done readBytes for package, read %d\r\n", actuallyRead);
00760     pcSerial.printf("verifyCode %d\r\n", verifyCode);
00761     
00762     fwrite( packageBody, 1, actuallyRead, jpgFile );
00763     
00764     pcSerial.printf("done fwrite\r\n" );
00765 
00766     
00767     return actuallyRead;
00768 
00769 }    
00770 
00771 uint32_t UCam::readData()
00772 {
00773     uint8_t bytes[6];
00774     
00775     if( 6 != readBytes( bytes, 6))
00776     {
00777         pcSerial.printf("readData failed to read 6 bytes\r\n");
00778         m_confused = 1;
00779         return 0;
00780     }
00781 
00782     uint32_t totalSize = ( bytes[5]<<16 ) | ( bytes[4]<<8 ) | ( bytes[3] );
00783 
00784     // check content - AA 0A tt nn nn nn - tt is the image type, nn tells us the image size
00785 
00786     // only log fail cases here, otherwise the logging slows us down
00787     // and we miss the image data, which is coming along already
00788     
00789     if( bytes[0] != 0xAA || bytes[1] != 0x0A)
00790     {
00791         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] );
00792         
00793          pcSerial.printf("readData failed\r\n");
00794          m_confused = 1;
00795          return 0;
00796     }
00797         
00798     return totalSize;
00799 }
00800