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