Richard Sewell
This is the code we showed at Uncraftivism
ucam.cpp@1:70d90598d2e7, 2009-12-10 (annotated)
- Committer:
- jarkman
- Date:
- Thu Dec 10 21:16:57 2009 +0000
- Revision:
- 1:70d90598d2e7
- Parent:
- 0:57f4fdadc97f
- Child:
- 2:01115080f6da
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jarkman | 0:57f4fdadc97f | 1 | #include "stdafx.h" |
| jarkman | 0:57f4fdadc97f | 2 | |
| jarkman | 0:57f4fdadc97f | 3 | #include "mbed.h" |
| jarkman | 0:57f4fdadc97f | 4 | #include "ucam.h" |
| jarkman | 0:57f4fdadc97f | 5 | #include "Frame.h" |
| jarkman | 0:57f4fdadc97f | 6 | #include "ServoMinder.h" |
| jarkman | 0:57f4fdadc97f | 7 | #include "MotionFinder.h" |
| jarkman | 0:57f4fdadc97f | 8 | #include "Servo.h" |
| jarkman | 0:57f4fdadc97f | 9 | #include "SerialBuffered.h" |
| jarkman | 0:57f4fdadc97f | 10 | |
| jarkman | 0:57f4fdadc97f | 11 | |
| jarkman | 0:57f4fdadc97f | 12 | // ucam protocol implementation for mbed |
| jarkman | 0:57f4fdadc97f | 13 | |
| jarkman | 0:57f4fdadc97f | 14 | |
| jarkman | 0:57f4fdadc97f | 15 | Logger pcSerial(USBTX, USBRX); // tx, rx |
| jarkman | 0:57f4fdadc97f | 16 | |
| jarkman | 0:57f4fdadc97f | 17 | |
| jarkman | 0:57f4fdadc97f | 18 | |
| jarkman | 0:57f4fdadc97f | 19 | DigitalOut myled1(LED1); |
| jarkman | 0:57f4fdadc97f | 20 | DigitalOut myled2(LED2); |
| jarkman | 0:57f4fdadc97f | 21 | DigitalOut myled3(LED3); |
| jarkman | 0:57f4fdadc97f | 22 | DigitalOut myled4(LED4); |
| jarkman | 0:57f4fdadc97f | 23 | |
| jarkman | 0:57f4fdadc97f | 24 | |
| jarkman | 0:57f4fdadc97f | 25 | |
| jarkman | 0:57f4fdadc97f | 26 | |
| jarkman | 0:57f4fdadc97f | 27 | //LocalFileSystem local("local"); |
| jarkman | 0:57f4fdadc97f | 28 | UCam ucam(p13, p14); |
| jarkman | 0:57f4fdadc97f | 29 | |
| jarkman | 0:57f4fdadc97f | 30 | |
| jarkman | 0:57f4fdadc97f | 31 | |
| jarkman | 0:57f4fdadc97f | 32 | Servo xServo (p21); |
| jarkman | 0:57f4fdadc97f | 33 | Servo yServo (p22); |
| jarkman | 1:70d90598d2e7 | 34 | Servo eyelidServo (p23); |
| jarkman | 1:70d90598d2e7 | 35 | #define EYE_CLOSED 0.15 |
| jarkman | 1:70d90598d2e7 | 36 | #define EYE_OPEN 0.8 |
| jarkman | 0:57f4fdadc97f | 37 | |
| jarkman | 0:57f4fdadc97f | 38 | MotionFinder *motionFinder = NULL; |
| jarkman | 0:57f4fdadc97f | 39 | |
| jarkman | 0:57f4fdadc97f | 40 | |
| jarkman | 1:70d90598d2e7 | 41 | void testEyelid() |
| jarkman | 1:70d90598d2e7 | 42 | { |
| jarkman | 1:70d90598d2e7 | 43 | ServoMinder *eyelidMinder = new ServoMinder( &eyelidServo ); |
| jarkman | 0:57f4fdadc97f | 44 | |
| jarkman | 1:70d90598d2e7 | 45 | eyelidMinder->setSpeed( 1 ); |
| jarkman | 1:70d90598d2e7 | 46 | eyelidMinder->moveToAndWait( EYE_CLOSED ); |
| jarkman | 1:70d90598d2e7 | 47 | |
| jarkman | 1:70d90598d2e7 | 48 | eyelidMinder->setSpeed( 0.1 ); |
| jarkman | 1:70d90598d2e7 | 49 | eyelidMinder->moveToAndWait( EYE_OPEN ); |
| jarkman | 1:70d90598d2e7 | 50 | eyelidMinder->setSpeed( 2 ); |
| jarkman | 1:70d90598d2e7 | 51 | eyelidMinder->moveToAndWait( EYE_CLOSED ); |
| jarkman | 1:70d90598d2e7 | 52 | eyelidMinder->moveToAndWait( EYE_OPEN ); |
| jarkman | 1:70d90598d2e7 | 53 | } |
| jarkman | 1:70d90598d2e7 | 54 | |
| jarkman | 0:57f4fdadc97f | 55 | void UCamInit() { |
| jarkman | 0:57f4fdadc97f | 56 | |
| jarkman | 1:70d90598d2e7 | 57 | |
| jarkman | 1:70d90598d2e7 | 58 | |
| jarkman | 0:57f4fdadc97f | 59 | ucam.doStartup(); |
| jarkman | 0:57f4fdadc97f | 60 | Frame::initFrames(); |
| jarkman | 1:70d90598d2e7 | 61 | |
| jarkman | 1:70d90598d2e7 | 62 | |
| jarkman | 1:70d90598d2e7 | 63 | testEyelid(); |
| jarkman | 1:70d90598d2e7 | 64 | |
| jarkman | 0:57f4fdadc97f | 65 | motionFinder = new MotionFinder( new ServoMinder(&xServo), new ServoMinder(&yServo) ); |
| jarkman | 0:57f4fdadc97f | 66 | |
| jarkman | 0:57f4fdadc97f | 67 | } |
| jarkman | 0:57f4fdadc97f | 68 | |
| jarkman | 1:70d90598d2e7 | 69 | |
| jarkman | 0:57f4fdadc97f | 70 | void UCamGetJpeg() |
| jarkman | 0:57f4fdadc97f | 71 | { |
| jarkman | 0:57f4fdadc97f | 72 | ucam.doConfig( false, UCAM_COLOUR_JPEG, UCAM_JPEG_SIZE_640x480 ); |
| jarkman | 0:57f4fdadc97f | 73 | |
| jarkman | 0:57f4fdadc97f | 74 | |
| jarkman | 0:57f4fdadc97f | 75 | uint8_t picType = UCAM_PIC_TYPE_JPEG_PREVIEW; |
| jarkman | 0:57f4fdadc97f | 76 | |
| jarkman | 0:57f4fdadc97f | 77 | |
| jarkman | 0:57f4fdadc97f | 78 | |
| jarkman | 0:57f4fdadc97f | 79 | ucam.doGetJpegPictureToFile( picType, "C:/mbed/out.jpg" ); |
| jarkman | 0:57f4fdadc97f | 80 | |
| jarkman | 0:57f4fdadc97f | 81 | } |
| jarkman | 0:57f4fdadc97f | 82 | |
| jarkman | 0:57f4fdadc97f | 83 | Frame* UCamGetRaw( ) |
| jarkman | 1:70d90598d2e7 | 84 | |
| jarkman | 0:57f4fdadc97f | 85 | { |
| jarkman | 0:57f4fdadc97f | 86 | ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60); |
| jarkman | 0:57f4fdadc97f | 87 | |
| jarkman | 0:57f4fdadc97f | 88 | return ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW ); // returns a frame which the caller must release |
| jarkman | 0:57f4fdadc97f | 89 | |
| jarkman | 0:57f4fdadc97f | 90 | } |
| jarkman | 0:57f4fdadc97f | 91 | |
| jarkman | 0:57f4fdadc97f | 92 | Frame* UCamGetDiff( ) |
| jarkman | 0:57f4fdadc97f | 93 | { |
| jarkman | 0:57f4fdadc97f | 94 | ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60 ); |
| jarkman | 0:57f4fdadc97f | 95 | |
| jarkman | 0:57f4fdadc97f | 96 | Frame *frame = ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW ); |
| jarkman | 0:57f4fdadc97f | 97 | |
| jarkman | 0:57f4fdadc97f | 98 | |
| jarkman | 0:57f4fdadc97f | 99 | motionFinder->processFrame( frame ); // returns a frame which the caller must *not* release |
| jarkman | 0:57f4fdadc97f | 100 | |
| jarkman | 0:57f4fdadc97f | 101 | return motionFinder->m_resultFrame; |
| jarkman | 0:57f4fdadc97f | 102 | |
| jarkman | 0:57f4fdadc97f | 103 | } |
| jarkman | 0:57f4fdadc97f | 104 | |
| jarkman | 0:57f4fdadc97f | 105 | Frame* UCamResetDiff( ) |
| jarkman | 0:57f4fdadc97f | 106 | { |
| jarkman | 0:57f4fdadc97f | 107 | ucam.doConfig( true, UCAM_COLOUR_4_BIT_GREY, UCAM_RAW_SIZE_80x60 ); |
| jarkman | 0:57f4fdadc97f | 108 | |
| jarkman | 0:57f4fdadc97f | 109 | Frame *frame = ucam.doGetRawPictureToBuffer( UCAM_PIC_TYPE_RAW_PREVIEW ); |
| jarkman | 0:57f4fdadc97f | 110 | |
| jarkman | 0:57f4fdadc97f | 111 | |
| jarkman | 0:57f4fdadc97f | 112 | motionFinder->newBackground( frame ); |
| jarkman | 0:57f4fdadc97f | 113 | |
| jarkman | 0:57f4fdadc97f | 114 | return motionFinder->m_resultFrame; // returns a frame which the caller must *not* release |
| jarkman | 0:57f4fdadc97f | 115 | |
| jarkman | 0:57f4fdadc97f | 116 | } |
| jarkman | 0:57f4fdadc97f | 117 | |
| jarkman | 0:57f4fdadc97f | 118 | UCam::UCam( PinName tx, PinName rx ) : camSerial(5000, p13, p14) // tx, rx |
| jarkman | 0:57f4fdadc97f | 119 | //UCam::UCam( PinName tx, PinName rx ) : camSerial( p13, p14) // tx, rx |
| jarkman | 0:57f4fdadc97f | 120 | |
| jarkman | 0:57f4fdadc97f | 121 | { |
| jarkman | 0:57f4fdadc97f | 122 | lastCommand = 0; |
| jarkman | 0:57f4fdadc97f | 123 | m_confused = 0; |
| jarkman | 0:57f4fdadc97f | 124 | m_colourType = UCAM_COLOUR_NOT_SET; |
| jarkman | 0:57f4fdadc97f | 125 | camSerial.setTimeout( 1.0 ); |
| jarkman | 0:57f4fdadc97f | 126 | |
| jarkman | 0:57f4fdadc97f | 127 | } |
| jarkman | 0:57f4fdadc97f | 128 | |
| jarkman | 0:57f4fdadc97f | 129 | void UCam::doStartup() |
| jarkman | 0:57f4fdadc97f | 130 | { |
| jarkman | 0:57f4fdadc97f | 131 | pcSerial.printf("\r\n\n\nucam waiting\r\n"); |
| jarkman | 0:57f4fdadc97f | 132 | |
| jarkman | 0:57f4fdadc97f | 133 | |
| jarkman | 0:57f4fdadc97f | 134 | wait(5); //delay to give time to get the terminal emulator up & running |
| jarkman | 0:57f4fdadc97f | 135 | |
| jarkman | 0:57f4fdadc97f | 136 | pcSerial.printf("\r\n\n\nucam running!\r\n"); |
| jarkman | 0:57f4fdadc97f | 137 | |
| jarkman | 0:57f4fdadc97f | 138 | // 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) |
| jarkman | 0:57f4fdadc97f | 139 | #ifdef ON_DESKTOP |
| jarkman | 0:57f4fdadc97f | 140 | camSerial.baud(14400); // lowest supported rate |
| jarkman | 0:57f4fdadc97f | 141 | |
| jarkman | 0:57f4fdadc97f | 142 | #else |
| jarkman | 0:57f4fdadc97f | 143 | //camSerial.baud(14400); // lowest supported rate |
| jarkman | 0:57f4fdadc97f | 144 | |
| jarkman | 0:57f4fdadc97f | 145 | //camSerial.baud(57600); |
| jarkman | 0:57f4fdadc97f | 146 | |
| jarkman | 0:57f4fdadc97f | 147 | camSerial.baud(115200); // highest supported rate |
| jarkman | 0:57f4fdadc97f | 148 | #endif |
| jarkman | 0:57f4fdadc97f | 149 | |
| jarkman | 0:57f4fdadc97f | 150 | myled1 = 1; |
| jarkman | 0:57f4fdadc97f | 151 | |
| jarkman | 0:57f4fdadc97f | 152 | // drain pending bytes |
| jarkman | 0:57f4fdadc97f | 153 | pcSerial.printf("doStartup - draining\r\n"); |
| jarkman | 0:57f4fdadc97f | 154 | while( camSerial.readable()) |
| jarkman | 0:57f4fdadc97f | 155 | uint8_t a = camSerial.getc(); |
| jarkman | 0:57f4fdadc97f | 156 | |
| jarkman | 0:57f4fdadc97f | 157 | doConnect(); |
| jarkman | 0:57f4fdadc97f | 158 | |
| jarkman | 0:57f4fdadc97f | 159 | |
| jarkman | 0:57f4fdadc97f | 160 | |
| jarkman | 0:57f4fdadc97f | 161 | //wait 2 secs for camera to warm up |
| jarkman | 0:57f4fdadc97f | 162 | wait(2); |
| jarkman | 0:57f4fdadc97f | 163 | |
| jarkman | 0:57f4fdadc97f | 164 | myled1 = 0; |
| jarkman | 0:57f4fdadc97f | 165 | |
| jarkman | 0:57f4fdadc97f | 166 | pcSerial.printf("doStartup finished\r\n"); |
| jarkman | 0:57f4fdadc97f | 167 | } |
| jarkman | 0:57f4fdadc97f | 168 | |
| jarkman | 0:57f4fdadc97f | 169 | int UCam::doConfig( bool raw, uint8_t colourType, uint8_t imageSize ) |
| jarkman | 0:57f4fdadc97f | 170 | { |
| jarkman | 0:57f4fdadc97f | 171 | myled2 = 1; |
| jarkman | 0:57f4fdadc97f | 172 | |
| jarkman | 0:57f4fdadc97f | 173 | m_raw = raw; |
| jarkman | 0:57f4fdadc97f | 174 | m_colourType = colourType; |
| jarkman | 0:57f4fdadc97f | 175 | m_imageSize = imageSize; |
| jarkman | 0:57f4fdadc97f | 176 | |
| jarkman | 0:57f4fdadc97f | 177 | // defaults |
| jarkman | 0:57f4fdadc97f | 178 | uint8_t rawSize = UCAM_RAW_SIZE_80x60; |
| jarkman | 0:57f4fdadc97f | 179 | uint8_t jpegSize = UCAM_JPEG_SIZE_80x64; |
| jarkman | 0:57f4fdadc97f | 180 | |
| jarkman | 0:57f4fdadc97f | 181 | |
| jarkman | 0:57f4fdadc97f | 182 | if( m_raw ) |
| jarkman | 0:57f4fdadc97f | 183 | { |
| jarkman | 0:57f4fdadc97f | 184 | switch( m_imageSize ) |
| jarkman | 0:57f4fdadc97f | 185 | { |
| jarkman | 0:57f4fdadc97f | 186 | // Sizes for raw images |
| jarkman | 0:57f4fdadc97f | 187 | case UCAM_RAW_SIZE_80x60: |
| jarkman | 0:57f4fdadc97f | 188 | rawSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 189 | m_width = 80; |
| jarkman | 0:57f4fdadc97f | 190 | m_height = 60; |
| jarkman | 0:57f4fdadc97f | 191 | break; |
| jarkman | 0:57f4fdadc97f | 192 | |
| jarkman | 0:57f4fdadc97f | 193 | case UCAM_RAW_SIZE_160x120: |
| jarkman | 0:57f4fdadc97f | 194 | rawSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 195 | m_width = 160; |
| jarkman | 0:57f4fdadc97f | 196 | m_height = 120; |
| jarkman | 0:57f4fdadc97f | 197 | break; |
| jarkman | 0:57f4fdadc97f | 198 | |
| jarkman | 0:57f4fdadc97f | 199 | case UCAM_RAW_SIZE_320x240: |
| jarkman | 0:57f4fdadc97f | 200 | rawSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 201 | m_width = 320; |
| jarkman | 0:57f4fdadc97f | 202 | m_height = 240; |
| jarkman | 0:57f4fdadc97f | 203 | break; |
| jarkman | 0:57f4fdadc97f | 204 | |
| jarkman | 0:57f4fdadc97f | 205 | case UCAM_RAW_SIZE_640x480: |
| jarkman | 0:57f4fdadc97f | 206 | rawSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 207 | m_width = 640; |
| jarkman | 0:57f4fdadc97f | 208 | m_height = 480; |
| jarkman | 0:57f4fdadc97f | 209 | break; |
| jarkman | 0:57f4fdadc97f | 210 | |
| jarkman | 0:57f4fdadc97f | 211 | case UCAM_RAW_SIZE_128x128: |
| jarkman | 0:57f4fdadc97f | 212 | rawSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 213 | m_width = 128; |
| jarkman | 0:57f4fdadc97f | 214 | m_height = 128; |
| jarkman | 0:57f4fdadc97f | 215 | break; |
| jarkman | 0:57f4fdadc97f | 216 | |
| jarkman | 0:57f4fdadc97f | 217 | case UCAM_RAW_SIZE_128x96: |
| jarkman | 0:57f4fdadc97f | 218 | default: |
| jarkman | 0:57f4fdadc97f | 219 | rawSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 220 | m_width = 128; |
| jarkman | 0:57f4fdadc97f | 221 | m_height = 96; |
| jarkman | 0:57f4fdadc97f | 222 | break; |
| jarkman | 0:57f4fdadc97f | 223 | } |
| jarkman | 0:57f4fdadc97f | 224 | } |
| jarkman | 0:57f4fdadc97f | 225 | else |
| jarkman | 0:57f4fdadc97f | 226 | { |
| jarkman | 0:57f4fdadc97f | 227 | // not raw - must be jpeg |
| jarkman | 0:57f4fdadc97f | 228 | switch( m_imageSize ) |
| jarkman | 0:57f4fdadc97f | 229 | { |
| jarkman | 0:57f4fdadc97f | 230 | |
| jarkman | 0:57f4fdadc97f | 231 | |
| jarkman | 0:57f4fdadc97f | 232 | case UCAM_JPEG_SIZE_80x64: |
| jarkman | 0:57f4fdadc97f | 233 | jpegSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 234 | m_width = 80; |
| jarkman | 0:57f4fdadc97f | 235 | m_height = 64; |
| jarkman | 0:57f4fdadc97f | 236 | break; |
| jarkman | 0:57f4fdadc97f | 237 | |
| jarkman | 0:57f4fdadc97f | 238 | case UCAM_JPEG_SIZE_160x128: |
| jarkman | 0:57f4fdadc97f | 239 | jpegSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 240 | m_width = 160; |
| jarkman | 0:57f4fdadc97f | 241 | m_height = 128; |
| jarkman | 0:57f4fdadc97f | 242 | break; |
| jarkman | 0:57f4fdadc97f | 243 | |
| jarkman | 0:57f4fdadc97f | 244 | case UCAM_JPEG_SIZE_320x240: |
| jarkman | 0:57f4fdadc97f | 245 | jpegSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 246 | m_width = 320; |
| jarkman | 0:57f4fdadc97f | 247 | m_height = 240; |
| jarkman | 0:57f4fdadc97f | 248 | break; |
| jarkman | 0:57f4fdadc97f | 249 | |
| jarkman | 0:57f4fdadc97f | 250 | case UCAM_JPEG_SIZE_640x480: |
| jarkman | 0:57f4fdadc97f | 251 | default: |
| jarkman | 0:57f4fdadc97f | 252 | jpegSize = m_imageSize; |
| jarkman | 0:57f4fdadc97f | 253 | m_width = 640; |
| jarkman | 0:57f4fdadc97f | 254 | m_height = 480; |
| jarkman | 0:57f4fdadc97f | 255 | break; |
| jarkman | 0:57f4fdadc97f | 256 | |
| jarkman | 0:57f4fdadc97f | 257 | } |
| jarkman | 0:57f4fdadc97f | 258 | |
| jarkman | 0:57f4fdadc97f | 259 | |
| jarkman | 0:57f4fdadc97f | 260 | |
| jarkman | 0:57f4fdadc97f | 261 | |
| jarkman | 0:57f4fdadc97f | 262 | } |
| jarkman | 0:57f4fdadc97f | 263 | |
| jarkman | 0:57f4fdadc97f | 264 | |
| jarkman | 0:57f4fdadc97f | 265 | pcSerial.printf("doConfig sending INITIAL %x %x %x\r\n", colourType, rawSize, jpegSize ); |
| jarkman | 0:57f4fdadc97f | 266 | |
| jarkman | 0:57f4fdadc97f | 267 | if( ! doCommand( UCAM_INITIAL, 0x00, |
| jarkman | 0:57f4fdadc97f | 268 | colourType, // colour type - 07 for jpg |
| jarkman | 0:57f4fdadc97f | 269 | rawSize, // raw resolution |
| jarkman | 0:57f4fdadc97f | 270 | jpegSize )) // jpeg resolution - 05 for 320x240 |
| jarkman | 0:57f4fdadc97f | 271 | return 0; |
| jarkman | 0:57f4fdadc97f | 272 | |
| jarkman | 0:57f4fdadc97f | 273 | pcSerial.printf("sending package size\r\n"); |
| jarkman | 0:57f4fdadc97f | 274 | |
| jarkman | 0:57f4fdadc97f | 275 | |
| jarkman | 0:57f4fdadc97f | 276 | // package size is only relevant for jpeg transfers |
| jarkman | 0:57f4fdadc97f | 277 | if( ! doCommand( UCAM_SET_PACKAGE_SIZE, 0x08, |
| jarkman | 0:57f4fdadc97f | 278 | 0x00, // low byte of size |
| jarkman | 0:57f4fdadc97f | 279 | 0x02, // high byte of size |
| jarkman | 0:57f4fdadc97f | 280 | 0x00 )) |
| jarkman | 0:57f4fdadc97f | 281 | return 0; |
| jarkman | 0:57f4fdadc97f | 282 | |
| jarkman | 0:57f4fdadc97f | 283 | |
| jarkman | 0:57f4fdadc97f | 284 | myled2 = 0; |
| jarkman | 0:57f4fdadc97f | 285 | |
| jarkman | 0:57f4fdadc97f | 286 | return 1; |
| jarkman | 0:57f4fdadc97f | 287 | } |
| jarkman | 0:57f4fdadc97f | 288 | |
| jarkman | 0:57f4fdadc97f | 289 | int UCam::fixConfusion() |
| jarkman | 0:57f4fdadc97f | 290 | { |
| jarkman | 0:57f4fdadc97f | 291 | if( ! m_confused ) |
| jarkman | 0:57f4fdadc97f | 292 | return 1; |
| jarkman | 0:57f4fdadc97f | 293 | |
| jarkman | 0:57f4fdadc97f | 294 | pcSerial.printf("fixConfusion - confused\r\n"); |
| jarkman | 0:57f4fdadc97f | 295 | for( int i = 0; i < 10; i ++ ) |
| jarkman | 0:57f4fdadc97f | 296 | { |
| jarkman | 0:57f4fdadc97f | 297 | // drain pending bytes |
| jarkman | 0:57f4fdadc97f | 298 | pcSerial.printf("fixConfusion - draining\r\n"); |
| jarkman | 0:57f4fdadc97f | 299 | while( camSerial.readable()) |
| jarkman | 0:57f4fdadc97f | 300 | uint8_t a = camSerial.getc(); |
| jarkman | 0:57f4fdadc97f | 301 | |
| jarkman | 0:57f4fdadc97f | 302 | // reset |
| jarkman | 0:57f4fdadc97f | 303 | pcSerial.printf("fixConfusion - resetting\r\n"); |
| jarkman | 0:57f4fdadc97f | 304 | |
| jarkman | 0:57f4fdadc97f | 305 | if( doReset()) |
| jarkman | 0:57f4fdadc97f | 306 | { |
| jarkman | 0:57f4fdadc97f | 307 | wait( 0.5 ); |
| jarkman | 0:57f4fdadc97f | 308 | // re-sync |
| jarkman | 0:57f4fdadc97f | 309 | if( doSyncs()) |
| jarkman | 0:57f4fdadc97f | 310 | { |
| jarkman | 0:57f4fdadc97f | 311 | |
| jarkman | 0:57f4fdadc97f | 312 | // re-config |
| jarkman | 0:57f4fdadc97f | 313 | |
| jarkman | 0:57f4fdadc97f | 314 | if( m_colourType == UCAM_COLOUR_NOT_SET || // for when we are confused before we have done any config |
| jarkman | 0:57f4fdadc97f | 315 | doConfig( m_raw, m_colourType, m_imageSize )) // for when we are confused after config, so ought to restore it |
| jarkman | 0:57f4fdadc97f | 316 | { |
| jarkman | 0:57f4fdadc97f | 317 | pcSerial.printf("fixConfusion - success\r\n"); |
| jarkman | 0:57f4fdadc97f | 318 | |
| jarkman | 0:57f4fdadc97f | 319 | m_confused = 0; |
| jarkman | 0:57f4fdadc97f | 320 | return 1; |
| jarkman | 0:57f4fdadc97f | 321 | } |
| jarkman | 0:57f4fdadc97f | 322 | } |
| jarkman | 0:57f4fdadc97f | 323 | |
| jarkman | 0:57f4fdadc97f | 324 | } |
| jarkman | 0:57f4fdadc97f | 325 | |
| jarkman | 0:57f4fdadc97f | 326 | pcSerial.printf("fixConfusion - trying again...\r\n"); |
| jarkman | 0:57f4fdadc97f | 327 | |
| jarkman | 0:57f4fdadc97f | 328 | } |
| jarkman | 0:57f4fdadc97f | 329 | |
| jarkman | 0:57f4fdadc97f | 330 | pcSerial.printf("fixConfusion - giving up\r\n"); |
| jarkman | 0:57f4fdadc97f | 331 | |
| jarkman | 0:57f4fdadc97f | 332 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 333 | return 0; |
| jarkman | 0:57f4fdadc97f | 334 | |
| jarkman | 0:57f4fdadc97f | 335 | } |
| jarkman | 0:57f4fdadc97f | 336 | |
| jarkman | 0:57f4fdadc97f | 337 | Frame* UCam::doGetRawPictureToBuffer( uint8_t pictureType ) |
| jarkman | 0:57f4fdadc97f | 338 | { |
| jarkman | 0:57f4fdadc97f | 339 | if( ! fixConfusion()) |
| jarkman | 0:57f4fdadc97f | 340 | return NULL; |
| jarkman | 0:57f4fdadc97f | 341 | |
| jarkman | 0:57f4fdadc97f | 342 | |
| jarkman | 0:57f4fdadc97f | 343 | if( pictureType == UCAM_PIC_TYPE_SNAPSHOT ) |
| jarkman | 0:57f4fdadc97f | 344 | doSnapshot( UCAM_SNAPSHOT_RAW ); |
| jarkman | 0:57f4fdadc97f | 345 | |
| jarkman | 0:57f4fdadc97f | 346 | pcSerial.printf("sending get picture\r\n"); |
| jarkman | 0:57f4fdadc97f | 347 | |
| jarkman | 0:57f4fdadc97f | 348 | myled3 = 1; |
| jarkman | 0:57f4fdadc97f | 349 | |
| jarkman | 0:57f4fdadc97f | 350 | if( ! doCommand( UCAM_GET_PICTURE, pictureType, 0x00, 0x00, 0x00 )) |
| jarkman | 0:57f4fdadc97f | 351 | { |
| jarkman | 0:57f4fdadc97f | 352 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 353 | pcSerial.printf("failed GET_PICTURE - giving up\r\n"); |
| jarkman | 0:57f4fdadc97f | 354 | return 0; |
| jarkman | 0:57f4fdadc97f | 355 | } |
| jarkman | 0:57f4fdadc97f | 356 | |
| jarkman | 0:57f4fdadc97f | 357 | |
| jarkman | 0:57f4fdadc97f | 358 | |
| jarkman | 0:57f4fdadc97f | 359 | uint32_t totalBytes = readData(); |
| jarkman | 0:57f4fdadc97f | 360 | |
| jarkman | 0:57f4fdadc97f | 361 | if( totalBytes < 1 ) |
| jarkman | 0:57f4fdadc97f | 362 | { |
| jarkman | 0:57f4fdadc97f | 363 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 364 | pcSerial.printf("totalBytes < 1 - giving up\r\n"); |
| jarkman | 0:57f4fdadc97f | 365 | return 0; |
| jarkman | 0:57f4fdadc97f | 366 | } |
| jarkman | 0:57f4fdadc97f | 367 | |
| jarkman | 0:57f4fdadc97f | 368 | |
| jarkman | 0:57f4fdadc97f | 369 | Frame *frame = NULL; |
| jarkman | 0:57f4fdadc97f | 370 | Frame::allocFrame( &frame, m_colourType, m_width, m_height, totalBytes ); |
| jarkman | 0:57f4fdadc97f | 371 | |
| jarkman | 0:57f4fdadc97f | 372 | if( frame == NULL || frame->m_bad ) |
| jarkman | 0:57f4fdadc97f | 373 | { |
| jarkman | 0:57f4fdadc97f | 374 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 375 | pcSerial.printf("doGetRawPictureToBuffer - bad frame - giving up\r\n"); |
| jarkman | 0:57f4fdadc97f | 376 | return 0; |
| jarkman | 0:57f4fdadc97f | 377 | } |
| jarkman | 0:57f4fdadc97f | 378 | |
| jarkman | 0:57f4fdadc97f | 379 | uint8_t *rawBuffer = frame->m_pixels; |
| jarkman | 0:57f4fdadc97f | 380 | |
| jarkman | 0:57f4fdadc97f | 381 | |
| jarkman | 0:57f4fdadc97f | 382 | uint32_t actuallyRead = readBytes( rawBuffer, totalBytes ); |
| jarkman | 0:57f4fdadc97f | 383 | |
| jarkman | 0:57f4fdadc97f | 384 | |
| jarkman | 0:57f4fdadc97f | 385 | |
| jarkman | 0:57f4fdadc97f | 386 | sendAckForRawData(); |
| jarkman | 0:57f4fdadc97f | 387 | |
| jarkman | 0:57f4fdadc97f | 388 | if( actuallyRead < totalBytes ) |
| jarkman | 0:57f4fdadc97f | 389 | { |
| jarkman | 0:57f4fdadc97f | 390 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 391 | Frame::releaseFrame( &frame ); |
| jarkman | 0:57f4fdadc97f | 392 | pcSerial.printf("Not enough bytes - %d < %d \r\n", (int) actuallyRead, (int) totalBytes ); |
| jarkman | 0:57f4fdadc97f | 393 | return NULL; |
| jarkman | 0:57f4fdadc97f | 394 | } |
| jarkman | 0:57f4fdadc97f | 395 | |
| jarkman | 0:57f4fdadc97f | 396 | |
| jarkman | 0:57f4fdadc97f | 397 | pcSerial.printf("Done!\r\n"); |
| jarkman | 0:57f4fdadc97f | 398 | |
| jarkman | 0:57f4fdadc97f | 399 | myled3 = 0; |
| jarkman | 0:57f4fdadc97f | 400 | |
| jarkman | 0:57f4fdadc97f | 401 | return frame; |
| jarkman | 0:57f4fdadc97f | 402 | } |
| jarkman | 0:57f4fdadc97f | 403 | |
| jarkman | 0:57f4fdadc97f | 404 | |
| jarkman | 0:57f4fdadc97f | 405 | int UCam::doConnect() |
| jarkman | 0:57f4fdadc97f | 406 | { |
| jarkman | 0:57f4fdadc97f | 407 | while( true ) |
| jarkman | 0:57f4fdadc97f | 408 | { |
| jarkman | 0:57f4fdadc97f | 409 | if( doSyncs()) |
| jarkman | 0:57f4fdadc97f | 410 | { |
| jarkman | 0:57f4fdadc97f | 411 | break; |
| jarkman | 0:57f4fdadc97f | 412 | } |
| jarkman | 0:57f4fdadc97f | 413 | else |
| jarkman | 0:57f4fdadc97f | 414 | { |
| jarkman | 0:57f4fdadc97f | 415 | m_confused = true; |
| jarkman | 0:57f4fdadc97f | 416 | if( fixConfusion()) |
| jarkman | 0:57f4fdadc97f | 417 | break; |
| jarkman | 0:57f4fdadc97f | 418 | } |
| jarkman | 0:57f4fdadc97f | 419 | } |
| jarkman | 0:57f4fdadc97f | 420 | |
| jarkman | 0:57f4fdadc97f | 421 | return 1; |
| jarkman | 0:57f4fdadc97f | 422 | } |
| jarkman | 0:57f4fdadc97f | 423 | |
| jarkman | 0:57f4fdadc97f | 424 | int UCam::doSyncs() |
| jarkman | 0:57f4fdadc97f | 425 | { |
| jarkman | 0:57f4fdadc97f | 426 | int i = 0; |
| jarkman | 0:57f4fdadc97f | 427 | |
| jarkman | 0:57f4fdadc97f | 428 | while( true ) |
| jarkman | 0:57f4fdadc97f | 429 | { |
| jarkman | 0:57f4fdadc97f | 430 | pcSerial.printf("sending sync\r\n"); |
| jarkman | 0:57f4fdadc97f | 431 | |
| jarkman | 0:57f4fdadc97f | 432 | wait( 0.5 ); |
| jarkman | 0:57f4fdadc97f | 433 | |
| jarkman | 0:57f4fdadc97f | 434 | sendCommand( UCAM_SYNC, 0x00, 0x00, 0x00, 0x00 ); |
| jarkman | 0:57f4fdadc97f | 435 | |
| jarkman | 0:57f4fdadc97f | 436 | //// pcSerial.printf("sent sync\r\n"); |
| jarkman | 0:57f4fdadc97f | 437 | |
| jarkman | 0:57f4fdadc97f | 438 | |
| jarkman | 0:57f4fdadc97f | 439 | if( camSerial.readable()) |
| jarkman | 0:57f4fdadc97f | 440 | { |
| jarkman | 0:57f4fdadc97f | 441 | //pcSerial.printf("readable - trying ack\r\n"); |
| jarkman | 0:57f4fdadc97f | 442 | |
| jarkman | 0:57f4fdadc97f | 443 | if( readAckPatiently(UCAM_SYNC)) |
| jarkman | 0:57f4fdadc97f | 444 | break; |
| jarkman | 0:57f4fdadc97f | 445 | } |
| jarkman | 0:57f4fdadc97f | 446 | |
| jarkman | 0:57f4fdadc97f | 447 | if( i ++ > 10 ) |
| jarkman | 0:57f4fdadc97f | 448 | return 0; |
| jarkman | 0:57f4fdadc97f | 449 | |
| jarkman | 0:57f4fdadc97f | 450 | } |
| jarkman | 0:57f4fdadc97f | 451 | |
| jarkman | 0:57f4fdadc97f | 452 | if( ! readSync()) |
| jarkman | 0:57f4fdadc97f | 453 | return 0; |
| jarkman | 0:57f4fdadc97f | 454 | |
| jarkman | 0:57f4fdadc97f | 455 | myled1 = 1; |
| jarkman | 0:57f4fdadc97f | 456 | |
| jarkman | 0:57f4fdadc97f | 457 | |
| jarkman | 0:57f4fdadc97f | 458 | sendAck(); |
| jarkman | 0:57f4fdadc97f | 459 | |
| jarkman | 0:57f4fdadc97f | 460 | return 1; |
| jarkman | 0:57f4fdadc97f | 461 | } |
| jarkman | 0:57f4fdadc97f | 462 | |
| jarkman | 0:57f4fdadc97f | 463 | void UCam::sendCommand( int command, int p1, int p2, int p3, int p4 ) |
| jarkman | 0:57f4fdadc97f | 464 | { |
| jarkman | 0:57f4fdadc97f | 465 | camSerial.putc( (command >> 8) & 0xff ); |
| jarkman | 0:57f4fdadc97f | 466 | camSerial.putc( command & 0xff ); |
| jarkman | 0:57f4fdadc97f | 467 | camSerial.putc( p1 ); |
| jarkman | 0:57f4fdadc97f | 468 | camSerial.putc( p2 ); |
| jarkman | 0:57f4fdadc97f | 469 | camSerial.putc( p3 ); |
| jarkman | 0:57f4fdadc97f | 470 | camSerial.putc( p4 ); |
| jarkman | 0:57f4fdadc97f | 471 | |
| jarkman | 0:57f4fdadc97f | 472 | |
| jarkman | 0:57f4fdadc97f | 473 | } |
| jarkman | 0:57f4fdadc97f | 474 | |
| jarkman | 0:57f4fdadc97f | 475 | int UCam::doCommand( int command, int p1, int p2, int p3, int p4 ) |
| jarkman | 0:57f4fdadc97f | 476 | { |
| jarkman | 0:57f4fdadc97f | 477 | sendCommand( command, p1,p2, p3, p4 ); |
| jarkman | 0:57f4fdadc97f | 478 | |
| jarkman | 0:57f4fdadc97f | 479 | |
| jarkman | 0:57f4fdadc97f | 480 | return readAck( command ); |
| jarkman | 0:57f4fdadc97f | 481 | } |
| jarkman | 0:57f4fdadc97f | 482 | |
| jarkman | 0:57f4fdadc97f | 483 | int UCam::doReset() |
| jarkman | 0:57f4fdadc97f | 484 | { |
| jarkman | 0:57f4fdadc97f | 485 | return doCommand( UCAM_RESET, |
| jarkman | 0:57f4fdadc97f | 486 | 0x00, // 0x00 reboots camera |
| jarkman | 0:57f4fdadc97f | 487 | //0x01, // 0x01 resets state machines, does not reboot camera |
| jarkman | 0:57f4fdadc97f | 488 | 0x00, 0x00, |
| jarkman | 0:57f4fdadc97f | 489 | 0xFF ); // 00 is a regular reset, FF causes a 'special reset' which is more immediate |
| jarkman | 0:57f4fdadc97f | 490 | |
| jarkman | 0:57f4fdadc97f | 491 | |
| jarkman | 0:57f4fdadc97f | 492 | |
| jarkman | 0:57f4fdadc97f | 493 | } |
| jarkman | 0:57f4fdadc97f | 494 | |
| jarkman | 0:57f4fdadc97f | 495 | int UCam::doSnapshot( uint8_t snapshotType ) |
| jarkman | 0:57f4fdadc97f | 496 | { |
| jarkman | 0:57f4fdadc97f | 497 | return doCommand( UCAM_SNAPSHOT, snapshotType, 0x00, 0x00, 0x00 ); |
| jarkman | 0:57f4fdadc97f | 498 | } |
| jarkman | 0:57f4fdadc97f | 499 | |
| jarkman | 0:57f4fdadc97f | 500 | void UCam::sendAck() |
| jarkman | 0:57f4fdadc97f | 501 | { |
| jarkman | 0:57f4fdadc97f | 502 | sendCommand( UCAM_ACK, 0x0D, 0x00, 0x00, 0x00 ); |
| jarkman | 0:57f4fdadc97f | 503 | } |
| jarkman | 0:57f4fdadc97f | 504 | |
| jarkman | 0:57f4fdadc97f | 505 | void UCam::sendAckForPackage( uint16_t p) // requests the camera to send the data for the package with that number |
| jarkman | 0:57f4fdadc97f | 506 | { |
| jarkman | 0:57f4fdadc97f | 507 | sendCommand( UCAM_ACK, 0x00, 0x00, p & 0xff, (p >> 8) & 0xff); |
| jarkman | 0:57f4fdadc97f | 508 | } |
| jarkman | 0:57f4fdadc97f | 509 | |
| jarkman | 0:57f4fdadc97f | 510 | void UCam::sendAckForRawData( ) |
| jarkman | 0:57f4fdadc97f | 511 | { |
| jarkman | 0:57f4fdadc97f | 512 | sendCommand( UCAM_ACK, 0x00, 0x0A, 0x01, 0x00); |
| jarkman | 0:57f4fdadc97f | 513 | } |
| jarkman | 0:57f4fdadc97f | 514 | |
| jarkman | 0:57f4fdadc97f | 515 | int UCam::readAck( uint16_t command ) |
| jarkman | 0:57f4fdadc97f | 516 | { |
| jarkman | 0:57f4fdadc97f | 517 | |
| jarkman | 0:57f4fdadc97f | 518 | uint8_t bytes[6]; |
| jarkman | 0:57f4fdadc97f | 519 | |
| jarkman | 0:57f4fdadc97f | 520 | readBytes( bytes, 6); |
| jarkman | 0:57f4fdadc97f | 521 | |
| jarkman | 0:57f4fdadc97f | 522 | // 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] ); |
| jarkman | 0:57f4fdadc97f | 523 | |
| jarkman | 0:57f4fdadc97f | 524 | if( bytes[0] != 0xaa || bytes[1] != 0x0e || bytes[2] != (command & 0xff)) |
| jarkman | 0:57f4fdadc97f | 525 | { |
| jarkman | 0:57f4fdadc97f | 526 | 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] ); |
| jarkman | 0:57f4fdadc97f | 527 | |
| jarkman | 0:57f4fdadc97f | 528 | if( bytes[1] == 0x0f ) |
| jarkman | 0:57f4fdadc97f | 529 | pcSerial.printf("ack is a NAK, error code %x for command %x\r\n", (int) bytes[4], (int) command); |
| jarkman | 0:57f4fdadc97f | 530 | else |
| jarkman | 0:57f4fdadc97f | 531 | pcSerial.printf("ack is for wrong command! Should be for %x\r\n", (int) command); |
| jarkman | 0:57f4fdadc97f | 532 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 533 | return 0; |
| jarkman | 0:57f4fdadc97f | 534 | } |
| jarkman | 0:57f4fdadc97f | 535 | |
| jarkman | 0:57f4fdadc97f | 536 | return 1; |
| jarkman | 0:57f4fdadc97f | 537 | } |
| jarkman | 0:57f4fdadc97f | 538 | |
| jarkman | 0:57f4fdadc97f | 539 | int UCam::readAckPatiently( uint16_t command ) |
| jarkman | 0:57f4fdadc97f | 540 | { |
| jarkman | 0:57f4fdadc97f | 541 | uint8_t a = 0; |
| jarkman | 0:57f4fdadc97f | 542 | uint16_t n = 0; |
| jarkman | 0:57f4fdadc97f | 543 | |
| jarkman | 0:57f4fdadc97f | 544 | while( n < 100 || camSerial.readable()) // used when we are waiting for a response to a sync, when we need to skip garbage bytes |
| jarkman | 0:57f4fdadc97f | 545 | { |
| jarkman | 0:57f4fdadc97f | 546 | a = camSerial.getc(); |
| jarkman | 0:57f4fdadc97f | 547 | if( a == 0xAA ) |
| jarkman | 0:57f4fdadc97f | 548 | { |
| jarkman | 0:57f4fdadc97f | 549 | // pcSerial.printf("ack read AA\r\n"); |
| jarkman | 0:57f4fdadc97f | 550 | |
| jarkman | 0:57f4fdadc97f | 551 | break; |
| jarkman | 0:57f4fdadc97f | 552 | } |
| jarkman | 0:57f4fdadc97f | 553 | |
| jarkman | 0:57f4fdadc97f | 554 | n++; |
| jarkman | 0:57f4fdadc97f | 555 | |
| jarkman | 0:57f4fdadc97f | 556 | pcSerial.printf("ackPatiently skipped %x\r\n", (int) a); |
| jarkman | 0:57f4fdadc97f | 557 | } |
| jarkman | 0:57f4fdadc97f | 558 | |
| jarkman | 0:57f4fdadc97f | 559 | uint8_t bytes[5]; |
| jarkman | 0:57f4fdadc97f | 560 | |
| jarkman | 0:57f4fdadc97f | 561 | readBytes( bytes, 5); |
| jarkman | 0:57f4fdadc97f | 562 | |
| jarkman | 0:57f4fdadc97f | 563 | |
| jarkman | 0:57f4fdadc97f | 564 | if( a != 0xaa || bytes[1] != (command & 0xff)) |
| jarkman | 0:57f4fdadc97f | 565 | { |
| jarkman | 0:57f4fdadc97f | 566 | 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] ); |
| jarkman | 0:57f4fdadc97f | 567 | |
| jarkman | 0:57f4fdadc97f | 568 | pcSerial.printf("ackPatiently is for wrong command! Should be for %x\r\n", (int) command); |
| jarkman | 0:57f4fdadc97f | 569 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 570 | return 0; |
| jarkman | 0:57f4fdadc97f | 571 | } |
| jarkman | 0:57f4fdadc97f | 572 | else |
| jarkman | 0:57f4fdadc97f | 573 | { |
| jarkman | 0:57f4fdadc97f | 574 | pcSerial.printf("ackPatiently is good!\r\n"); |
| jarkman | 0:57f4fdadc97f | 575 | } |
| jarkman | 0:57f4fdadc97f | 576 | |
| jarkman | 0:57f4fdadc97f | 577 | return 1; |
| jarkman | 0:57f4fdadc97f | 578 | } |
| jarkman | 0:57f4fdadc97f | 579 | |
| jarkman | 0:57f4fdadc97f | 580 | int UCam::readSync() |
| jarkman | 0:57f4fdadc97f | 581 | { |
| jarkman | 0:57f4fdadc97f | 582 | uint8_t bytes[6]; |
| jarkman | 0:57f4fdadc97f | 583 | |
| jarkman | 0:57f4fdadc97f | 584 | readBytes( bytes,6 ); |
| jarkman | 0:57f4fdadc97f | 585 | |
| jarkman | 0:57f4fdadc97f | 586 | // check content |
| jarkman | 0:57f4fdadc97f | 587 | |
| jarkman | 0:57f4fdadc97f | 588 | |
| jarkman | 0:57f4fdadc97f | 589 | if( bytes[0] != 0xAA || bytes[1] != 0x0D || bytes[2] != 0x00 || bytes[3] != 0x00 || bytes[4] != 0x00 || bytes[5] != 0x00 ) |
| jarkman | 0:57f4fdadc97f | 590 | { |
| jarkman | 0:57f4fdadc97f | 591 | 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] ); |
| jarkman | 0:57f4fdadc97f | 592 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 593 | return 0; |
| jarkman | 0:57f4fdadc97f | 594 | } |
| jarkman | 0:57f4fdadc97f | 595 | return 1; |
| jarkman | 0:57f4fdadc97f | 596 | } |
| jarkman | 0:57f4fdadc97f | 597 | |
| jarkman | 0:57f4fdadc97f | 598 | |
| jarkman | 0:57f4fdadc97f | 599 | |
| jarkman | 0:57f4fdadc97f | 600 | uint16_t UCam::readUInt16() |
| jarkman | 0:57f4fdadc97f | 601 | { |
| jarkman | 0:57f4fdadc97f | 602 | uint8_t bytes[2]; |
| jarkman | 0:57f4fdadc97f | 603 | |
| jarkman | 0:57f4fdadc97f | 604 | readBytes( bytes, 2); |
| jarkman | 0:57f4fdadc97f | 605 | |
| jarkman | 0:57f4fdadc97f | 606 | // pcSerial.printf("readUInt16 read %x %x \r\n", (int) bytes[0], (int) bytes[1] ); |
| jarkman | 0:57f4fdadc97f | 607 | |
| jarkman | 0:57f4fdadc97f | 608 | |
| jarkman | 0:57f4fdadc97f | 609 | uint16_t i = bytes[1]<<8 | bytes[0]; |
| jarkman | 0:57f4fdadc97f | 610 | |
| jarkman | 0:57f4fdadc97f | 611 | |
| jarkman | 0:57f4fdadc97f | 612 | |
| jarkman | 0:57f4fdadc97f | 613 | return i; |
| jarkman | 0:57f4fdadc97f | 614 | } |
| jarkman | 0:57f4fdadc97f | 615 | |
| jarkman | 0:57f4fdadc97f | 616 | int UCam::readBytes(uint8_t *bytes, int size ) |
| jarkman | 0:57f4fdadc97f | 617 | { |
| jarkman | 0:57f4fdadc97f | 618 | |
| jarkman | 0:57f4fdadc97f | 619 | int n = camSerial.readBytes( bytes, size ); |
| jarkman | 0:57f4fdadc97f | 620 | if( n < size ) |
| jarkman | 0:57f4fdadc97f | 621 | { |
| jarkman | 0:57f4fdadc97f | 622 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 623 | int m = n; |
| jarkman | 0:57f4fdadc97f | 624 | |
| jarkman | 0:57f4fdadc97f | 625 | // put some zeroes in the output to make clear it's empty |
| jarkman | 0:57f4fdadc97f | 626 | do |
| jarkman | 0:57f4fdadc97f | 627 | { |
| jarkman | 0:57f4fdadc97f | 628 | bytes[m] = (uint8_t) 0; |
| jarkman | 0:57f4fdadc97f | 629 | m ++; |
| jarkman | 0:57f4fdadc97f | 630 | } |
| jarkman | 0:57f4fdadc97f | 631 | while( m < size && m < 20 ); |
| jarkman | 0:57f4fdadc97f | 632 | |
| jarkman | 0:57f4fdadc97f | 633 | |
| jarkman | 0:57f4fdadc97f | 634 | } |
| jarkman | 0:57f4fdadc97f | 635 | |
| jarkman | 0:57f4fdadc97f | 636 | return n; |
| jarkman | 0:57f4fdadc97f | 637 | |
| jarkman | 0:57f4fdadc97f | 638 | |
| jarkman | 0:57f4fdadc97f | 639 | } |
| jarkman | 0:57f4fdadc97f | 640 | |
| jarkman | 0:57f4fdadc97f | 641 | |
| jarkman | 0:57f4fdadc97f | 642 | |
| jarkman | 0:57f4fdadc97f | 643 | |
| jarkman | 0:57f4fdadc97f | 644 | |
| jarkman | 0:57f4fdadc97f | 645 | int UCam::doGetJpegPictureToFile( uint8_t pictureType, char*filename ) |
| jarkman | 0:57f4fdadc97f | 646 | { |
| jarkman | 0:57f4fdadc97f | 647 | if( ! fixConfusion()) |
| jarkman | 0:57f4fdadc97f | 648 | return NULL; |
| jarkman | 0:57f4fdadc97f | 649 | |
| jarkman | 0:57f4fdadc97f | 650 | if( pictureType == UCAM_PIC_TYPE_SNAPSHOT ) |
| jarkman | 0:57f4fdadc97f | 651 | doSnapshot( UCAM_SNAPSHOT_JPEG ); |
| jarkman | 0:57f4fdadc97f | 652 | |
| jarkman | 0:57f4fdadc97f | 653 | FILE *jpgFile = fopen(filename, FILE_WRITE_STRING); // "w" or "wb" for Windows |
| jarkman | 0:57f4fdadc97f | 654 | |
| jarkman | 0:57f4fdadc97f | 655 | if( jpgFile != NULL ) |
| jarkman | 0:57f4fdadc97f | 656 | pcSerial.printf("opened output file\r\n"); |
| jarkman | 0:57f4fdadc97f | 657 | |
| jarkman | 0:57f4fdadc97f | 658 | pcSerial.printf("sending get picture\r\n"); |
| jarkman | 0:57f4fdadc97f | 659 | |
| jarkman | 0:57f4fdadc97f | 660 | myled3 = 1; |
| jarkman | 0:57f4fdadc97f | 661 | |
| jarkman | 0:57f4fdadc97f | 662 | doCommand( UCAM_GET_PICTURE, pictureType, 0x00, 0x00, 0x00 ); |
| jarkman | 0:57f4fdadc97f | 663 | |
| jarkman | 0:57f4fdadc97f | 664 | pcSerial.printf("sent get_picture\r\n"); |
| jarkman | 0:57f4fdadc97f | 665 | |
| jarkman | 0:57f4fdadc97f | 666 | uint32_t totalBytes = readData(); |
| jarkman | 0:57f4fdadc97f | 667 | |
| jarkman | 0:57f4fdadc97f | 668 | // pcSerial.printf("totalBytes is %d bytes\r\n", (int) totalBytes ); |
| jarkman | 0:57f4fdadc97f | 669 | |
| jarkman | 0:57f4fdadc97f | 670 | uint16_t packageN = 0; |
| jarkman | 0:57f4fdadc97f | 671 | |
| jarkman | 0:57f4fdadc97f | 672 | |
| jarkman | 0:57f4fdadc97f | 673 | uint32_t bytesRead = 0; |
| jarkman | 0:57f4fdadc97f | 674 | |
| jarkman | 0:57f4fdadc97f | 675 | while( bytesRead < totalBytes ) |
| jarkman | 0:57f4fdadc97f | 676 | { |
| jarkman | 0:57f4fdadc97f | 677 | sendAckForPackage(packageN); |
| jarkman | 0:57f4fdadc97f | 678 | |
| jarkman | 0:57f4fdadc97f | 679 | int actuallyRead = readPackage( jpgFile, packageN ); |
| jarkman | 0:57f4fdadc97f | 680 | |
| jarkman | 0:57f4fdadc97f | 681 | pcSerial.printf("read package of %d bytes\r\n", (int) actuallyRead ); |
| jarkman | 0:57f4fdadc97f | 682 | |
| jarkman | 0:57f4fdadc97f | 683 | if( actuallyRead < 0 ) |
| jarkman | 0:57f4fdadc97f | 684 | { |
| jarkman | 0:57f4fdadc97f | 685 | pcSerial.printf("didn't read enough bytes of package - giving up\r\n"); |
| jarkman | 0:57f4fdadc97f | 686 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 687 | |
| jarkman | 0:57f4fdadc97f | 688 | break; |
| jarkman | 0:57f4fdadc97f | 689 | } |
| jarkman | 0:57f4fdadc97f | 690 | |
| jarkman | 0:57f4fdadc97f | 691 | bytesRead += actuallyRead; |
| jarkman | 0:57f4fdadc97f | 692 | |
| jarkman | 0:57f4fdadc97f | 693 | packageN++; |
| jarkman | 0:57f4fdadc97f | 694 | |
| jarkman | 0:57f4fdadc97f | 695 | |
| jarkman | 0:57f4fdadc97f | 696 | |
| jarkman | 0:57f4fdadc97f | 697 | } |
| jarkman | 0:57f4fdadc97f | 698 | |
| jarkman | 0:57f4fdadc97f | 699 | sendAckForPackage(0xF0F0); |
| jarkman | 0:57f4fdadc97f | 700 | |
| jarkman | 0:57f4fdadc97f | 701 | fclose( jpgFile ); |
| jarkman | 0:57f4fdadc97f | 702 | |
| jarkman | 0:57f4fdadc97f | 703 | pcSerial.printf("Done!\r\n"); |
| jarkman | 0:57f4fdadc97f | 704 | |
| jarkman | 0:57f4fdadc97f | 705 | myled3 = 0; |
| jarkman | 0:57f4fdadc97f | 706 | return 1; |
| jarkman | 0:57f4fdadc97f | 707 | } |
| jarkman | 0:57f4fdadc97f | 708 | |
| jarkman | 0:57f4fdadc97f | 709 | |
| jarkman | 0:57f4fdadc97f | 710 | int UCam::readPackage( FILE *jpgFile, uint16_t targetPackage ) |
| jarkman | 0:57f4fdadc97f | 711 | { |
| jarkman | 0:57f4fdadc97f | 712 | int actuallyRead; |
| jarkman | 0:57f4fdadc97f | 713 | uint16_t packageId; |
| jarkman | 0:57f4fdadc97f | 714 | // 2 bytes id |
| jarkman | 0:57f4fdadc97f | 715 | packageId = readUInt16(); |
| jarkman | 0:57f4fdadc97f | 716 | |
| jarkman | 0:57f4fdadc97f | 717 | |
| jarkman | 0:57f4fdadc97f | 718 | |
| jarkman | 0:57f4fdadc97f | 719 | //pcSerial.printf("packageId is %d\r\n", (int)packageId ); |
| jarkman | 0:57f4fdadc97f | 720 | |
| jarkman | 0:57f4fdadc97f | 721 | if( packageId != targetPackage ) |
| jarkman | 0:57f4fdadc97f | 722 | { |
| jarkman | 0:57f4fdadc97f | 723 | pcSerial.printf("bad package id %d (%x) in package header for target id %d - giving up\r\n", packageId, packageId, targetPackage); |
| jarkman | 0:57f4fdadc97f | 724 | |
| jarkman | 0:57f4fdadc97f | 725 | actuallyRead = readBytes( packageBody, 500 ); |
| jarkman | 0:57f4fdadc97f | 726 | pcSerial.printf("next %d bytes\r\n", actuallyRead); |
| jarkman | 0:57f4fdadc97f | 727 | for( int i = 0 ; i < actuallyRead; i ++ ) |
| jarkman | 0:57f4fdadc97f | 728 | pcSerial.printf("%x\r\n", packageBody[i]); |
| jarkman | 0:57f4fdadc97f | 729 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 730 | return -1; |
| jarkman | 0:57f4fdadc97f | 731 | } |
| jarkman | 0:57f4fdadc97f | 732 | |
| jarkman | 0:57f4fdadc97f | 733 | // 2 bytes data size |
| jarkman | 0:57f4fdadc97f | 734 | uint16_t packageSize = readUInt16(); |
| jarkman | 0:57f4fdadc97f | 735 | |
| jarkman | 0:57f4fdadc97f | 736 | //pcSerial.printf("packageSize is %d bytes\r\n", (int)packageSize ); |
| jarkman | 0:57f4fdadc97f | 737 | |
| jarkman | 0:57f4fdadc97f | 738 | |
| jarkman | 0:57f4fdadc97f | 739 | int dataSize = packageSize; // - 6; |
| jarkman | 0:57f4fdadc97f | 740 | |
| jarkman | 0:57f4fdadc97f | 741 | //pcSerial.printf("dataSize is %d bytes\r\n", (int)dataSize ); |
| jarkman | 0:57f4fdadc97f | 742 | |
| jarkman | 0:57f4fdadc97f | 743 | if( dataSize > sizeof( packageBody )) // too big - give up |
| jarkman | 0:57f4fdadc97f | 744 | { |
| jarkman | 0:57f4fdadc97f | 745 | pcSerial.printf("bad dataSize %d in package header for id %d - giving up\r\n", dataSize, packageId); |
| jarkman | 0:57f4fdadc97f | 746 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 747 | return -1; |
| jarkman | 0:57f4fdadc97f | 748 | } |
| jarkman | 0:57f4fdadc97f | 749 | |
| jarkman | 0:57f4fdadc97f | 750 | // image data (package size - 6 bytes) |
| jarkman | 0:57f4fdadc97f | 751 | actuallyRead = readBytes( packageBody, dataSize); |
| jarkman | 0:57f4fdadc97f | 752 | |
| jarkman | 0:57f4fdadc97f | 753 | |
| jarkman | 0:57f4fdadc97f | 754 | |
| jarkman | 0:57f4fdadc97f | 755 | //pcSerial.printf("done readBytes, read %d\r\n", actuallyRead); |
| jarkman | 0:57f4fdadc97f | 756 | |
| jarkman | 0:57f4fdadc97f | 757 | if( actuallyRead < dataSize ) |
| jarkman | 0:57f4fdadc97f | 758 | { |
| jarkman | 0:57f4fdadc97f | 759 | pcSerial.printf("bad readBytes, read %d\r\n", actuallyRead); |
| jarkman | 0:57f4fdadc97f | 760 | for( int i = 0 ; i < actuallyRead; i ++ ) |
| jarkman | 0:57f4fdadc97f | 761 | pcSerial.printf("%x\r\n", packageBody[i]); |
| jarkman | 0:57f4fdadc97f | 762 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 763 | return -1; |
| jarkman | 0:57f4fdadc97f | 764 | } |
| jarkman | 0:57f4fdadc97f | 765 | |
| jarkman | 0:57f4fdadc97f | 766 | // 2 bytes verify code |
| jarkman | 0:57f4fdadc97f | 767 | uint16_t verifyCode = readUInt16(); |
| jarkman | 0:57f4fdadc97f | 768 | |
| jarkman | 0:57f4fdadc97f | 769 | pcSerial.printf("done readBytes for package, read %d\r\n", actuallyRead); |
| jarkman | 0:57f4fdadc97f | 770 | pcSerial.printf("verifyCode %d\r\n", verifyCode); |
| jarkman | 0:57f4fdadc97f | 771 | |
| jarkman | 0:57f4fdadc97f | 772 | fwrite( packageBody, 1, actuallyRead, jpgFile ); |
| jarkman | 0:57f4fdadc97f | 773 | |
| jarkman | 0:57f4fdadc97f | 774 | pcSerial.printf("done fwrite\r\n" ); |
| jarkman | 0:57f4fdadc97f | 775 | |
| jarkman | 0:57f4fdadc97f | 776 | |
| jarkman | 0:57f4fdadc97f | 777 | return actuallyRead; |
| jarkman | 0:57f4fdadc97f | 778 | |
| jarkman | 0:57f4fdadc97f | 779 | } |
| jarkman | 0:57f4fdadc97f | 780 | |
| jarkman | 0:57f4fdadc97f | 781 | uint32_t UCam::readData() |
| jarkman | 0:57f4fdadc97f | 782 | { |
| jarkman | 0:57f4fdadc97f | 783 | uint8_t bytes[6]; |
| jarkman | 0:57f4fdadc97f | 784 | |
| jarkman | 0:57f4fdadc97f | 785 | if( 6 != readBytes( bytes, 6)) |
| jarkman | 0:57f4fdadc97f | 786 | { |
| jarkman | 0:57f4fdadc97f | 787 | pcSerial.printf("readData failed to read 6 bytes\r\n"); |
| jarkman | 0:57f4fdadc97f | 788 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 789 | return 0; |
| jarkman | 0:57f4fdadc97f | 790 | } |
| jarkman | 0:57f4fdadc97f | 791 | |
| jarkman | 0:57f4fdadc97f | 792 | uint32_t totalSize = ( bytes[5]<<16 ) | ( bytes[4]<<8 ) | ( bytes[3] ); |
| jarkman | 0:57f4fdadc97f | 793 | |
| jarkman | 0:57f4fdadc97f | 794 | // check content - AA 0A tt nn nn nn - tt is the image type, nn tells us the image size |
| jarkman | 0:57f4fdadc97f | 795 | |
| jarkman | 0:57f4fdadc97f | 796 | // only log fail cases here, otherwise the logging slows us down |
| jarkman | 0:57f4fdadc97f | 797 | // and we miss the image data, which is coming along already |
| jarkman | 0:57f4fdadc97f | 798 | |
| jarkman | 0:57f4fdadc97f | 799 | if( bytes[0] != 0xAA || bytes[1] != 0x0A) |
| jarkman | 0:57f4fdadc97f | 800 | { |
| jarkman | 0:57f4fdadc97f | 801 | 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] ); |
| jarkman | 0:57f4fdadc97f | 802 | |
| jarkman | 0:57f4fdadc97f | 803 | pcSerial.printf("readData failed\r\n"); |
| jarkman | 0:57f4fdadc97f | 804 | m_confused = 1; |
| jarkman | 0:57f4fdadc97f | 805 | return 0; |
| jarkman | 0:57f4fdadc97f | 806 | } |
| jarkman | 0:57f4fdadc97f | 807 | |
| jarkman | 0:57f4fdadc97f | 808 | return totalSize; |
| jarkman | 0:57f4fdadc97f | 809 | } |
| jarkman | 0:57f4fdadc97f | 810 |