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