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