CameraOV528.cpp

00001 /* Copyright (c) 2016 ARM Limited
00002  *
00003  * Licensed under the Apache License, Version 2.0 (the "License");
00004  * you may not use this file except in compliance with the License.
00005  * You may obtain a copy of the License at
00006  *
00007  *     http://www.apache.org/licenses/LICENSE-2.0
00008  *
00009  * Unless required by applicable law or agreed to in writing, software
00010  * distributed under the License is distributed on an "AS IS" BASIS,
00011  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00012  * See the License for the specific language governing permissions and
00013  * limitations under the License.
00014  */
00015 
00016 /*
00017  * Originally Based on the (Grove_Serial_Camera_Kit) - https://github.com/Seeed-Studio/Grove_Serial_Camera_Kit
00018  */
00019 
00020 #include<stdint.h>
00021 #include<stdlib.h>
00022 #include<string.h>
00023 
00024 #include "CameraOV528.h"
00025 
00026 #define camera_printf(...)
00027 
00028 #define PIC_CLOCK_HZ                14745600
00029 #define BOOT_BAUD                   9600
00030 
00031 #define COMMAND_LENGTH              6
00032 #define PIC_PAYLOAD_DATA_BEGIN      4
00033 #define PIC_PAYLOAD_OVERHEAD        6
00034 #define READ_WAKE_POS_INVALID       0xFFFFFFFF
00035 
00036 #define DEFAULT_BAUD                115200
00037 
00038 enum CameraCommand {
00039     INITIAL = 0x01,
00040     GET_PICTURE = 0x04,
00041     SNAPSHOT = 0x05,
00042     SET_PACKAGE_SIZE = 0x06,
00043     SET_BAUD_RATE = 0x07,
00044     RESET_CAMERA = 0x08,
00045     POWER_DOWN = 0x09,
00046     DATA = 0x0A,
00047     SYNC = 0x0D,
00048     ACK = 0x0E,
00049     NAK = 0x0F
00050 };
00051 
00052 enum GetSetting {
00053     GET_SNAPSHOT = 1,
00054     GET_PREVIEW_PICTURE = 2,
00055     GET_JPEG_PREVIEW_PICTURE = 3,
00056 };
00057 
00058 typedef struct {
00059     uint8_t header;
00060     uint8_t command;
00061     uint8_t param[4];
00062 } camera_command_t;
00063 
00064 CameraOV528::Resolution supported_resolutions[] = {
00065 //    CameraOV528::RES_80x60,             //Does not work
00066     CameraOV528::RES_160x120,
00067     CameraOV528::RES_320x240,
00068     CameraOV528::RES_640x480,
00069 };
00070 
00071 CameraOV528::Format supported_formats[] = {
00072 //    CameraOV528::FMT_2_BIT_GRAY_SCALE,  //Does not work
00073 //    CameraOV528::FMT_4_BIT_GRAY_SCALE,
00074 //    CameraOV528::FMT_8_BIT_GRAY_SCALE,
00075 //    CameraOV528::FMT_2_BIT_COLOR,
00076 //    CameraOV528::FMT_16_BIT,
00077     CameraOV528::FMT_JPEG,
00078 };
00079 
00080 static uint32_t divide_round_up(uint32_t dividen, uint32_t divisor);
00081 static uint32_t min(uint32_t value1, uint32_t value2);
00082 static uint32_t queue_used(uint32_t head, uint32_t tail, uint32_t size);
00083 
00084 CameraOV528::CameraOV528(PinName rx, PinName tx) : _serial(rx, tx), _read_sem(0)
00085 {
00086     _init_done = false;
00087     _resolution = RES_640x480;
00088     _format = FMT_JPEG;
00089     _baud = DEFAULT_BAUD;
00090 
00091     _dev_resolution = _resolution;
00092     _dev_format = _format;
00093     _dev_baud = _baud;
00094 
00095     picture_length = 0;
00096     picture_data_id = 0;
00097     picture_data_id_count = 0;
00098     memset(picture_buffer, 0, sizeof(picture_buffer));
00099     picture_buffer_size_limit = sizeof(picture_buffer);
00100     picture_buffer_pos = 0;
00101     picture_buffer_size = 0;
00102 
00103     memset(_read_buf, 0, sizeof(_read_buf));
00104     _read_buf_head = 0;
00105     _read_buf_tail = 0;
00106     _read_wake_pos = READ_WAKE_POS_INVALID;
00107     this->log_func = debug;
00108 }
00109 
00110 CameraOV528::~CameraOV528()
00111 {
00112     powerdown();
00113 }
00114 
00115 void CameraOV528::attach_debug_function(void (*func)(const char* fmt, ...))
00116 {
00117     this->log_func = func;
00118 }
00119 
00120 int CameraOV528::camera_error(const char* message)
00121 {
00122     log_func("%s\r\n",message);
00123     return -1;
00124 }
00125 
00126 int CameraOV528::powerup()
00127 {
00128     if (_init_done) {
00129         return 0;
00130     }
00131 
00132     _serial.attach(this, &CameraOV528::_rx_irq, SerialBase::RxIrq);
00133 
00134     // Attempt to connect at the desired baud
00135      _serial.baud(_baud);
00136      bool success = _init_sequence();
00137 
00138      // If unsuccessful try with boot baud
00139     if (!success) {
00140         _serial.baud(BOOT_BAUD);
00141         success = _init_sequence();
00142     }
00143 
00144     if (!success) {
00145         return camera_error("Unable to communicate with camera");
00146     }
00147 
00148     // Acknowledge the SYNC read in _init_sequence with an ACK
00149     _send_ack(SYNC, 0, 0, 0);
00150     camera_printf("\nCamera initialization done.\r\n");
00151 
00152     _set_baud(_baud);
00153 
00154     _set_fmt_and_res(_format, _resolution);
00155     _set_package_size(picture_buffer_size_limit);
00156 
00157     _init_done = false;
00158     return 0;
00159 }
00160 
00161 int CameraOV528::powerup(uint32_t baud)
00162 {
00163     _baud = baud;
00164     return powerup();
00165 }
00166 
00167 int CameraOV528::powerdown()
00168 {
00169     if (!_init_done) {
00170         return 0;
00171     }
00172 
00173     if (!_send_cmd(POWER_DOWN, 0)) {
00174         return camera_error("Powerdown failed");
00175     }
00176 
00177     // Reset picture transfer variables
00178     picture_length = 0;
00179     picture_data_id = 0;
00180     picture_data_id_count = 0;
00181     memset(picture_buffer, 0, sizeof(picture_buffer));
00182     picture_buffer_size_limit = sizeof(picture_buffer);
00183     picture_buffer_pos = 0;
00184     picture_buffer_size = 0;
00185 
00186     _serial.attach(NULL, SerialBase::RxIrq);
00187     _flush_rx();
00188 
00189     _init_done = false;
00190     return 0;
00191 }
00192 
00193 int CameraOV528::take_picture(void)
00194 {
00195     // Ensure driver is powered up
00196     powerup();
00197 
00198     // Update settings if any have changed
00199     if ((_dev_format != _format) || (_dev_resolution != _resolution)) {
00200         _set_fmt_and_res(_format, _resolution);
00201         _dev_format = _format;
00202         _dev_resolution = _resolution;
00203     }
00204 
00205     // Take snapshot
00206     camera_printf("Taking snapshot\r\n");
00207     if (!_send_cmd(SNAPSHOT, 0)) {
00208         return camera_error("Take snapshot failed");
00209     }
00210 
00211     // Start picture transfer
00212     camera_printf("Starting transfer\r\n");
00213     const GetSetting request = GET_JPEG_PREVIEW_PICTURE;
00214     if (!_send_cmd(GET_PICTURE, request)) {
00215         return camera_error("Get picture command failed");
00216     }
00217     camera_command_t resp = {0};
00218     uint32_t size_read = _read((uint8_t*)&resp, COMMAND_LENGTH, 1000);
00219     if (size_read != COMMAND_LENGTH) {
00220         return camera_error("Get picture response invalid");
00221     }
00222     if (resp.header != 0xAA)  camera_error("Get picture response invalid sync");
00223     if (resp.command != DATA)  camera_error("Get picture response invalid data");
00224     if (resp.param[0] != request)  camera_error("Get picture response invalid content");
00225     picture_length = (resp.param[1] << 0) |
00226              (resp.param[2] << 8) |
00227              (resp.param[3] << 16);
00228     picture_data_id = 0;
00229     uint32_t payload_length = picture_buffer_size_limit - PIC_PAYLOAD_OVERHEAD;
00230     picture_data_id_count = divide_round_up(picture_length, payload_length);
00231     return 0;
00232 }
00233 
00234 uint32_t CameraOV528::get_picture_size()
00235 {
00236     return picture_length;
00237 }
00238 
00239 uint32_t CameraOV528::read_picture_data(uint8_t * data, uint32_t size)
00240 {
00241     uint32_t size_copied = 0;
00242     while (size_copied < size) {
00243 
00244         // Copy any data in the picture buffer
00245         uint32_t size_left = size - size_copied;
00246         uint32_t copy_size = min(picture_buffer_size, size_left);
00247         memcpy(data + size_copied, picture_buffer + picture_buffer_pos, copy_size);
00248         picture_buffer_pos += copy_size;
00249         picture_buffer_size -= copy_size;
00250         size_copied += copy_size;
00251 
00252         // If picture buffer is empty read more data
00253         if (0 == picture_buffer_size) {
00254             _read_picture_block();
00255         }
00256 
00257         // If there is still no more data to read then break from loop
00258         if (0 == picture_buffer_size) {
00259             break;
00260         }
00261     }
00262 
00263     return size_copied;
00264 }
00265 
00266 void CameraOV528::set_resolution(CameraOV528::Resolution resolution)
00267 {
00268     const uint32_t count = sizeof(supported_resolutions) / sizeof(supported_resolutions[0]);
00269     bool valid_resolution = false;
00270     for (uint32_t i = 0; i < count; i++) {
00271         if (resolution == supported_resolutions[i]) {
00272             valid_resolution = true;
00273             break;
00274         }
00275     }
00276 
00277     if (!valid_resolution) {
00278         camera_error("Invalid resolution");
00279     }
00280 
00281     _resolution = resolution;
00282 
00283 }
00284 
00285 void CameraOV528::set_format(CameraOV528::Format format)
00286 {
00287     const uint32_t count = sizeof(supported_formats) / sizeof(supported_formats[0]);
00288     bool valid_format = false;
00289     for (uint32_t i = 0; i < count; i++) {
00290         if (format == supported_formats[i]) {
00291             valid_format = true;
00292             break;
00293         }
00294     }
00295 
00296     if (!valid_format) {
00297         camera_error("Invalid format");
00298     }
00299 
00300     _format = format;
00301 }
00302 
00303 void CameraOV528::_set_baud(uint32_t baud)
00304 {
00305     // Baud Rate = 14.7456MHz/(2*(2nd divider +1))/(2*(1st divider+1))
00306     uint32_t div2 = 1;
00307     uint32_t div1 = PIC_CLOCK_HZ / _baud / (2 * (div2 + 1)) / 2 - 1;
00308     // Assert no rounding errors
00309     MBED_ASSERT(PIC_CLOCK_HZ / ( 2 * (div2 + 1) ) / ( 2 * (div1 + 1)) == _baud);
00310     if (!_send_cmd(SET_BAUD_RATE, div1, div2)) {
00311         camera_error("_set_baud failed");
00312     }
00313     _serial.baud(_baud);
00314 }
00315 
00316 void CameraOV528::_set_package_size(uint32_t size)
00317 {
00318     camera_printf("_set_package_size(%lu)\r\n", size);
00319     uint8_t size_low = (size >> 0) & 0xff;
00320     uint8_t size_high = (size >> 8) & 0xff;
00321     if (!_send_cmd(SET_PACKAGE_SIZE, 0x08, size_low, size_high, 0)) {
00322         camera_error("_set_package_size failed");
00323     }
00324 }
00325 
00326 void CameraOV528::_set_fmt_and_res(Format fmt, Resolution res)
00327 {
00328     if (!_send_cmd(INITIAL, 0x00, _format, 0x00, _resolution)) {
00329         camera_error("_set_fmt_and_res failed");
00330     }
00331 }
00332 
00333 int CameraOV528::_read_picture_block()
00334 {
00335     const uint32_t payload_length = picture_buffer_size_limit - PIC_PAYLOAD_OVERHEAD;
00336     if (picture_data_id >= picture_data_id_count) {
00337         // Transfer complete
00338         return 0;
00339     }
00340 
00341     // Send an ACK to indicate that the next block id should be sent
00342     uint8_t id_low = (picture_data_id >> 0) & 0xff;
00343     uint8_t id_high = (picture_data_id >> 8) & 0xff;
00344     _send_ack(0x00, 0x00, id_low, id_high);
00345 
00346     // Read image data
00347     memset(picture_buffer, 0, sizeof(picture_buffer));
00348     uint32_t size_left = picture_length - payload_length * picture_data_id;
00349     uint32_t size_to_read = min(size_left, payload_length) + PIC_PAYLOAD_OVERHEAD;
00350     uint32_t size_read = _read(picture_buffer, size_to_read);
00351     if (size_read != size_to_read) {
00352         return camera_error("Image data protocol error");
00353     }
00354 
00355     // Validate checksum
00356     uint8_t checksum = 0;
00357     for (uint32_t i = 0; i < size_read - 2; i++) {
00358         checksum += picture_buffer[i];
00359     }
00360     if (picture_buffer[size_read - 2] != checksum) {
00361         return camera_error("Image data checksum failure");
00362     }
00363 
00364     // Update buffer information
00365     picture_buffer_pos = PIC_PAYLOAD_DATA_BEGIN;
00366     picture_buffer_size = size_read - PIC_PAYLOAD_OVERHEAD;
00367 
00368     // If this is the last packet then send the completion ACK
00369     if (picture_data_id + 1 == picture_data_id_count) {
00370         _send_ack(0x00, 0x00, 0xF0, 0xF0);
00371     }
00372 
00373     // Increment position
00374     camera_printf("%lu of %lu\r\n", picture_data_id + 1, picture_data_id_count);
00375     camera_printf("size left %lu\r\n",
00376                   picture_length - payload_length * picture_data_id +
00377                   PIC_PAYLOAD_OVERHEAD - size_read);
00378     picture_data_id++;
00379     return 0;
00380 }
00381 
00382 void CameraOV528::_send_ack(uint8_t p1, uint8_t p2, uint8_t p3, uint8_t p4)
00383 {
00384     camera_command_t cmd = {0};
00385     cmd.header = 0xAA;
00386     cmd.command = ACK;
00387     cmd.param[0] = p1;
00388     cmd.param[1] = p2;
00389     cmd.param[2] = p3;
00390     cmd.param[3] = p4;
00391     _send((uint8_t*)&cmd, COMMAND_LENGTH);
00392 }
00393 
00394 void CameraOV528::_rx_irq(void)
00395 {
00396     if(_serial.readable()) {
00397 
00398         // Check for overflow
00399         if (_read_buf_head + 1 == _read_buf_tail) {
00400             camera_error("RX buffer overflow");
00401         }
00402 
00403         // Add data
00404         _read_buf[_read_buf_head] = (uint8_t)_serial.getc();
00405         _read_buf_head = (_read_buf_head + 1) % sizeof(_read_buf);
00406 
00407         // Check if thread should be woken
00408         if (_read_buf_head == _read_wake_pos) {
00409             _read_sem.release();
00410             _read_wake_pos = READ_WAKE_POS_INVALID;
00411         }
00412     }
00413 }
00414 
00415 bool CameraOV528::_send(const uint8_t *data, uint32_t size, uint32_t timeout_ms)
00416 {
00417     for (uint32_t i = 0; i < size; i++) {
00418         _serial.putc(data[i]);
00419     }
00420     return true;
00421 }
00422 
00423 uint32_t CameraOV528::_read(uint8_t *data, uint32_t size, uint32_t timeout_ms)
00424 {
00425     MBED_ASSERT(size < sizeof(_read_buf));
00426     MBED_ASSERT(0 == _read_sem.wait(0));
00427 
00428     core_util_critical_section_enter();
00429 
00430     // Atomically set wakeup condition
00431     uint32_t size_available = queue_used(_read_buf_head, _read_buf_tail, sizeof(_read_buf));
00432     if (size_available >= size) {
00433         _read_wake_pos = READ_WAKE_POS_INVALID;
00434     } else {
00435         _read_wake_pos = (_read_buf_tail + size) % sizeof(_read_buf);
00436     }
00437 
00438     core_util_critical_section_exit();
00439 
00440     // Wait until the requested number of bytes are available
00441     if (_read_wake_pos != READ_WAKE_POS_INVALID) {
00442         int32_t tokens = _read_sem.wait(timeout_ms);
00443         if (tokens <= 0) {
00444             // Timeout occurred so make sure semaphore is cleared
00445             _read_wake_pos = READ_WAKE_POS_INVALID;
00446             _read_sem.wait(0);
00447         } else {
00448             // If the semaphore was signaled then the requested number of
00449             // bytes were read
00450             MBED_ASSERT(queue_used(_read_buf_head, _read_buf_tail, sizeof(_read_buf)) >= size);
00451         }
00452     }
00453 
00454     // Copy bytes
00455     size_available = queue_used(_read_buf_head, _read_buf_tail, sizeof(_read_buf));
00456     uint32_t read_size = min(size_available, size);
00457     for (uint32_t i = 0; i < read_size; i++) {
00458         data[i] = _read_buf[_read_buf_tail];
00459         _read_buf_tail = (_read_buf_tail + 1) % sizeof(_read_buf);
00460     }
00461 
00462     return read_size;
00463 }
00464 
00465 
00466 void CameraOV528::_flush_rx(void)
00467 {
00468 
00469     core_util_critical_section_enter();
00470     _read_buf_head = 0;
00471     _read_buf_tail = 0;
00472     _read_wake_pos = 0;
00473     core_util_critical_section_exit();
00474 }
00475 
00476 bool CameraOV528::_send_cmd(uint8_t cmd, uint8_t p1, uint8_t p2, uint8_t p3, uint8_t p4)
00477 {
00478     _flush_rx();
00479 
00480     camera_command_t command = {0};
00481     command.header = 0xAA;
00482     command.command = cmd;
00483     command.param[0] = p1;
00484     command.param[1] = p2;
00485     command.param[2] = p3;
00486     command.param[3] = p4;
00487     _send((uint8_t*)&command, COMMAND_LENGTH);
00488 
00489     camera_command_t resp = {0};
00490     uint32_t len = _read((uint8_t*)&resp, COMMAND_LENGTH);
00491     if (COMMAND_LENGTH != len) {
00492         camera_printf("Wrong command response length %lu\r\n", len);
00493     }
00494     if ((resp.header != 0xAA) || (resp.command != ACK) || (resp.param[0] != command.command)) {
00495         camera_printf("Invalid command: %i, %i, %i\r\n", resp.header, resp.command, resp.param[0]);
00496         return false;
00497     }
00498     return true;
00499 }
00500 
00501 bool CameraOV528::_init_sequence()
00502 {
00503     camera_printf("connecting to camera...");
00504     bool success = false;
00505     for (uint32_t i = 0; i < 4; i++) {
00506         camera_printf(".");
00507 
00508         // Send SYNC command repeatedly
00509         if (!_send_cmd(SYNC, 0, 0, 0, 0)) {
00510             continue;
00511         }
00512         // Device should send back SYNC command
00513         camera_command_t resp = {0};
00514         if (_read((uint8_t*)&resp, COMMAND_LENGTH, 500) != COMMAND_LENGTH) {
00515             continue;
00516         }
00517         if (resp.header != 0xAA) continue;
00518         if (resp.command != SYNC) continue;
00519         if (resp.param[0] != 0) continue;
00520         if (resp.param[1] != 0) continue;
00521         if (resp.param[2] != 0) continue;
00522         if (resp.param[3] != 0) continue;
00523         success = true;
00524         break;
00525     }
00526     return success;
00527 }
00528 
00529 static uint32_t divide_round_up(uint32_t dividen, uint32_t divisor)
00530 {
00531     return (dividen + divisor - 1) / divisor;
00532 }
00533 
00534 static uint32_t min(uint32_t value1, uint32_t value2)
00535 {
00536     return value1 < value2 ? value1 : value2;
00537 }
00538 
00539 static uint32_t queue_used(uint32_t head, uint32_t tail, uint32_t size)
00540 {
00541     if (head < tail) {
00542         return head + size - tail;
00543     } else {
00544         return head - tail;
00545     }
00546 }