├── CMakeLists.txt ├── example ├── CMakeLists.txt ├── main.c └── pico_sdk_import.cmake ├── include └── camera │ ├── format.h │ ├── camera.h │ └── ov7670.h ├── format.c ├── camera.pio ├── README.md ├── camera.c └── ov7670.c /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(camera INTERFACE) 2 | 3 | target_include_directories(camera INTERFACE 4 | ${CMAKE_CURRENT_LIST_DIR}/include 5 | ) 6 | 7 | target_sources(camera INTERFACE 8 | ${CMAKE_CURRENT_LIST_DIR}/camera.c 9 | ${CMAKE_CURRENT_LIST_DIR}/format.c 10 | ${CMAKE_CURRENT_LIST_DIR}/ov7670.c 11 | ) 12 | 13 | pico_generate_pio_header(camera ${CMAKE_CURRENT_LIST_DIR}/camera.pio) 14 | 15 | target_link_libraries(camera INTERFACE 16 | hardware_clocks 17 | hardware_dma 18 | hardware_irq 19 | hardware_pio 20 | ) 21 | -------------------------------------------------------------------------------- /example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.13) 2 | 3 | include(pico_sdk_import.cmake) 4 | 5 | project(camera_example C CXX ASM) 6 | 7 | set(CMAKE_C_STANDARD 11) 8 | set(CMAKE_CXX_STANDARD 17) 9 | 10 | pico_sdk_init() 11 | 12 | add_executable(camera_example main.c) 13 | 14 | pico_enable_stdio_usb(camera_example 1) 15 | 16 | pico_add_extra_outputs(camera_example) 17 | 18 | target_link_libraries(camera_example 19 | camera 20 | pico_stdlib 21 | hardware_i2c 22 | hardware_gpio 23 | ) 24 | 25 | add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/.. camera) 26 | -------------------------------------------------------------------------------- /include/camera/format.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2022 Brian Starkey 3 | * 4 | * SPDX-License-Identifier: BSD-3-Clause 5 | */ 6 | #ifndef __FORMAT_H__ 7 | #define __FORMAT_H__ 8 | 9 | #include 10 | 11 | #define FORMAT_CODE(a, b, c, d) (((uint32_t)(a) << 0) | ((uint32_t)(b) << 8) | ((uint32_t)(c) << 16) | ((uint32_t)(d) << 24)) 12 | 13 | #define FORMAT_YUYV FORMAT_CODE('Y', 'U', 'Y', 'V') 14 | #define FORMAT_RGB565 FORMAT_CODE('R', 'G', '1', '6') 15 | #define FORMAT_YUV422 FORMAT_CODE('Y', 'U', '1', '6') 16 | 17 | uint8_t format_num_planes(uint32_t format); 18 | uint8_t format_bytes_per_pixel(uint32_t format, uint8_t plane); 19 | uint8_t format_hsub(uint32_t format, uint8_t plane); 20 | uint32_t format_stride(uint32_t format, uint8_t plane, uint16_t width); 21 | uint32_t format_plane_size(uint32_t format, uint8_t plane, uint16_t width, uint16_t height); 22 | 23 | #endif /* __FORMAT_H__ */ 24 | -------------------------------------------------------------------------------- /format.c: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2022 Brian Starkey 3 | * 4 | * SPDX-License-Identifier: BSD-3-Clause 5 | */ 6 | #include "camera/format.h" 7 | 8 | uint8_t format_num_planes(uint32_t format) 9 | { 10 | switch (format) { 11 | case FORMAT_YUYV: 12 | /* Fallthrough */ 13 | case FORMAT_RGB565: 14 | return 1; 15 | case FORMAT_YUV422: 16 | return 3; 17 | default: 18 | return 0; 19 | } 20 | } 21 | 22 | uint8_t format_bytes_per_pixel(uint32_t format, uint8_t plane) 23 | { 24 | switch (format) { 25 | case FORMAT_YUYV: 26 | /* Fallthrough */ 27 | case FORMAT_RGB565: 28 | return 2; 29 | case FORMAT_YUV422: 30 | return 1; 31 | default: 32 | return 0; 33 | } 34 | } 35 | 36 | uint8_t format_hsub(uint32_t format, uint8_t plane) 37 | { 38 | switch (format) { 39 | case FORMAT_YUV422: 40 | return plane ? 2 : 1; 41 | default: 42 | return 1; 43 | } 44 | } 45 | 46 | uint32_t format_stride(uint32_t format, uint8_t plane, uint16_t width) 47 | { 48 | return format_bytes_per_pixel(format, plane) * width / format_hsub(format, plane); 49 | } 50 | 51 | uint32_t format_plane_size(uint32_t format, uint8_t plane, uint16_t width, uint16_t height) 52 | { 53 | return format_stride(format, plane, width) * height; 54 | } 55 | -------------------------------------------------------------------------------- /example/main.c: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2022 Brian Starkey 3 | * 4 | * SPDX-License-Identifier: BSD-3-Clause 5 | */ 6 | 7 | #include 8 | 9 | #include "hardware/i2c.h" 10 | #include "pico/stdio.h" 11 | #include "pico/stdlib.h" 12 | 13 | #include "camera/camera.h" 14 | #include "camera/format.h" 15 | 16 | #define CAMERA_PIO pio0 17 | #define CAMERA_BASE_PIN 16 18 | #define CAMERA_XCLK_PIN 21 19 | #define CAMERA_SDA 0 20 | #define CAMERA_SCL 1 21 | 22 | static inline int __i2c_write_blocking(void *i2c_handle, uint8_t addr, const uint8_t *src, size_t len) 23 | { 24 | return i2c_write_blocking((i2c_inst_t *)i2c_handle, addr, src, len, false); 25 | } 26 | 27 | static inline int __i2c_read_blocking(void *i2c_handle, uint8_t addr, uint8_t *dst, size_t len) 28 | { 29 | return i2c_read_blocking((i2c_inst_t *)i2c_handle, addr, dst, len, false); 30 | } 31 | 32 | // From http://www.paulbourke.net/dataformats/asciiart/ 33 | const char charmap[] = " .:-=+*#%@"; 34 | 35 | int main() { 36 | stdio_init_all(); 37 | 38 | // Wait some time for USB serial connection 39 | sleep_ms(3000); 40 | 41 | const uint LED_PIN = PICO_DEFAULT_LED_PIN; 42 | gpio_init(LED_PIN); 43 | gpio_set_dir(LED_PIN, GPIO_OUT); 44 | 45 | i2c_init(i2c0, 100000); 46 | gpio_set_function(CAMERA_SDA, GPIO_FUNC_I2C); 47 | gpio_set_function(CAMERA_SCL, GPIO_FUNC_I2C); 48 | gpio_pull_up(CAMERA_SDA); 49 | gpio_pull_up(CAMERA_SCL); 50 | 51 | struct camera camera; 52 | struct camera_platform_config platform = { 53 | .i2c_write_blocking = __i2c_write_blocking, 54 | .i2c_read_blocking = __i2c_read_blocking, 55 | .i2c_handle = i2c0, 56 | 57 | .pio = CAMERA_PIO, 58 | .xclk_pin = CAMERA_XCLK_PIN, 59 | .xclk_divider = 9, 60 | .base_pin = CAMERA_BASE_PIN, 61 | .base_dma_channel = -1, 62 | }; 63 | 64 | int ret = camera_init(&camera, &platform); 65 | if (ret) { 66 | printf("camera_init failed: %d\n", ret); 67 | return 1; 68 | } 69 | 70 | const uint16_t width = CAMERA_WIDTH_DIV8; 71 | const uint16_t height = CAMERA_HEIGHT_DIV8; 72 | 73 | struct camera_buffer *buf = camera_buffer_alloc(FORMAT_YUV422, width, height); 74 | assert(buf); 75 | 76 | while (1) { 77 | printf("Capturing...\n"); 78 | gpio_put(LED_PIN, 1); 79 | ret = camera_capture_blocking(&camera, buf, true); 80 | gpio_put(LED_PIN, 0); 81 | if (ret != 0) { 82 | printf("Capture error: %d\n", ret); 83 | } else { 84 | printf("Capture success\n"); 85 | int y, x; 86 | for (y = 0; y < height; y++) { 87 | char row[width]; 88 | for (x = 0; x < height; x++) { 89 | uint8_t v = buf->data[0][buf->strides[0] * y + x]; 90 | v /= 26; 91 | row[x] = charmap[v]; 92 | } 93 | printf("%s\n", row); 94 | } 95 | } 96 | 97 | sleep_ms(5000); 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /example/pico_sdk_import.cmake: -------------------------------------------------------------------------------- 1 | # This is a copy of /external/pico_sdk_import.cmake 2 | 3 | # This can be dropped into an external project to help locate this SDK 4 | # It should be include()ed prior to project() 5 | 6 | if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH)) 7 | set(PICO_SDK_PATH $ENV{PICO_SDK_PATH}) 8 | message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')") 9 | endif () 10 | 11 | if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT)) 12 | set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT}) 13 | message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')") 14 | endif () 15 | 16 | if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH)) 17 | set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH}) 18 | message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')") 19 | endif () 20 | 21 | set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK") 22 | set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable") 23 | set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK") 24 | 25 | if (NOT PICO_SDK_PATH) 26 | if (PICO_SDK_FETCH_FROM_GIT) 27 | include(FetchContent) 28 | set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR}) 29 | if (PICO_SDK_FETCH_FROM_GIT_PATH) 30 | get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}") 31 | endif () 32 | FetchContent_Declare( 33 | pico_sdk 34 | GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk 35 | GIT_TAG master 36 | ) 37 | if (NOT pico_sdk) 38 | message("Downloading Raspberry Pi Pico SDK") 39 | FetchContent_Populate(pico_sdk) 40 | set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR}) 41 | endif () 42 | set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE}) 43 | else () 44 | message(FATAL_ERROR 45 | "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git." 46 | ) 47 | endif () 48 | endif () 49 | 50 | get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}") 51 | if (NOT EXISTS ${PICO_SDK_PATH}) 52 | message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found") 53 | endif () 54 | 55 | set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake) 56 | if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE}) 57 | message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK") 58 | endif () 59 | 60 | set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE) 61 | 62 | include(${PICO_SDK_INIT_CMAKE_FILE}) 63 | -------------------------------------------------------------------------------- /include/camera/camera.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2022 Brian Starkey 3 | * 4 | * SPDX-License-Identifier: BSD-3-Clause 5 | */ 6 | #ifndef __CAMERA_H__ 7 | #define __CAMERA_H__ 8 | 9 | #include 10 | 11 | #include "hardware/dma.h" 12 | #include "hardware/pio.h" 13 | 14 | #include "camera/ov7670.h" 15 | 16 | #define CAMERA_WIDTH_DIV8 80 17 | #define CAMERA_HEIGHT_DIV8 60 18 | 19 | #define CAMERA_MAX_N_PLANES 3 20 | 21 | // Stores a camera frame. 22 | // Can be allocated dynamically using camera_buffer_alloc/camera_buffer_free, 23 | // or you can use some static buffers and populate this structure manually. 24 | // The format_* helpers from format.h can help determine the correct values. 25 | struct camera_buffer { 26 | uint32_t format; 27 | uint16_t width; 28 | uint16_t height; 29 | uint32_t strides[CAMERA_MAX_N_PLANES]; 30 | uint32_t sizes[CAMERA_MAX_N_PLANES]; 31 | uint8_t *data[CAMERA_MAX_N_PLANES]; 32 | }; 33 | 34 | typedef void (*camera_frame_cb)(struct camera_buffer *buf, void *p); 35 | 36 | // Stores format/width/height dependent configuration for PIO and DMA 37 | struct camera_config { 38 | uint32_t format; 39 | uint16_t width; 40 | uint16_t height; 41 | uint dma_transfers[CAMERA_MAX_N_PLANES]; 42 | uint dma_offset[CAMERA_MAX_N_PLANES]; 43 | dma_channel_config dma_cfgs[CAMERA_MAX_N_PLANES]; 44 | pio_sm_config sm_cfgs[4]; 45 | }; 46 | 47 | struct camera { 48 | // Interface for the low level driver 49 | OV7670_host driver_host; 50 | 51 | // Static PIO and DMA configuration, set at camera_init time 52 | uint frame_offset; 53 | uint shift_byte_offset; 54 | int dma_channels[CAMERA_MAX_N_PLANES]; 55 | 56 | // Dynamic configuration set by width/height/format 57 | struct camera_config config; 58 | 59 | // Frame context, tracking the currently in-progress frame 60 | struct camera_buffer *volatile pending; 61 | camera_frame_cb volatile pending_cb; 62 | void *volatile cb_data; 63 | }; 64 | 65 | // Provides the platform abstraction for a camera instance. 66 | // Needs to be populated and provided to camera_init. 67 | struct camera_platform_config { 68 | // i2c access for configuring the sensor 69 | // Both functions should return the number of bytes transferred 70 | // (they should behave the same as the SDK i2c functions, with nostop = false) 71 | int (*i2c_write_blocking)(void *i2c_handle, uint8_t addr, const uint8_t *src, size_t len); 72 | int (*i2c_read_blocking)(void *i2c_handle, uint8_t addr, uint8_t *src, size_t len); 73 | void *i2c_handle; 74 | 75 | // PIO settings 76 | PIO pio; 77 | // xclk_pin must be one of the GPOUT clock pins: 21, 23, 24 or 25 78 | uint xclk_pin; 79 | uint xclk_divider; 80 | // See camera.pio for pin order 81 | uint base_pin; 82 | // 3 DMA channels will be claimed starting at base_dma_channel. 83 | // Set to -1 for dynamic assignment 84 | int base_dma_channel; 85 | }; 86 | 87 | // Initialise the camera. 88 | // The 'camera' structure doesn't need any initialisation, it's passed in only 89 | // to allow the caller to allocate it as they like. 90 | // 'params' must be populated by the caller, and must remain valid and in-scope 91 | // for the whole lifetime of the camera. 92 | // 93 | // Returns 0 on success 94 | int camera_init(struct camera *camera, struct camera_platform_config *params); 95 | 96 | // Not implemented, but theoretically would terminate the clock, clear the PIO 97 | // memory etc. 98 | void camera_term(struct camera *camera); 99 | 100 | // Configure the camera for a particular format/width/height. 101 | // This requires i2c interaction with the camera, which may be slow, so this 102 | // function exists to let users do that operation "up front" rather than 103 | // incurring an unknown delay at the point of triggering a capture. 104 | // 105 | // Returns 0 on success 106 | int camera_configure(struct camera *camera, uint32_t format, uint16_t width, uint16_t height); 107 | 108 | // Capture a frame into the provided buffer, blocking until it is complete. 109 | // 110 | // If the camera is already configured for the same format/width/height as 'into' 111 | // then no reconfiguration happens. 112 | // Otherwise, if allow_reconfigure is true then the camera will first be 113 | // reconfigured, and if not, capture will fail. 114 | // 115 | // Returns 0 on success 116 | int camera_capture_blocking(struct camera *camera, struct camera_buffer *into, bool allow_reconfigure); 117 | 118 | // Capture a frame into the provided buffer, calling complete_cb when complete. 119 | // Note: The callback is called from the PIO interrupt handler, so it must be 120 | // safe for use in interrupt context! 121 | // 122 | // If the camera is already configured for the same format/width/height as 'into' 123 | // then no reconfiguration happens. 124 | // Otherwise, if allow_reconfigure is true then the camera will first be 125 | // reconfigured, and if not, capture will fail. 126 | // 127 | // Returns 0 on success 128 | int camera_capture_with_cb(struct camera *camera, struct camera_buffer *into, bool allow_reconfigure, 129 | camera_frame_cb complete_cb, void *cb_data); 130 | 131 | // Allocate a camera buffer with the provided format, width and height 132 | // Uses malloc() to allocate the structure and the data buffers. 133 | struct camera_buffer *camera_buffer_alloc(uint32_t format, uint16_t width, uint16_t height); 134 | 135 | // Free a buffer previously allocated by camera_buffer_alloc. 136 | void camera_buffer_free(struct camera_buffer *buf); 137 | 138 | #endif /* __CAMERA_H__ */ 139 | -------------------------------------------------------------------------------- /camera.pio: -------------------------------------------------------------------------------- 1 | ; 2 | ; Copyright (c) 2021 Brian Starkey 3 | ; 4 | ; SPDX-License-Identifier: BSD-3-Clause 5 | ; 6 | 7 | .define PUBLIC PIN_OFFS_D0 0 8 | .define PUBLIC PIN_OFFS_CLK 1 9 | .define PUBLIC PIN_OFFS_VSYNC 2 10 | .define PUBLIC PIN_OFFS_HREF 3 11 | .define PUBLIC PIN_OFFS_PXCLK 4 12 | 13 | .define BYTE_IRQ_BASE 4 14 | 15 | .program camera_pio_shift_byte 16 | .side_set 1 opt 17 | 18 | .wrap_target 19 | set X 7 ; Load the bit counter 20 | wait 1 irq BYTE_IRQ_BASE rel ; Wait to be triggered - waits for IRQ 4 + SM number 21 | wait 1 pin PIN_OFFS_PXCLK [5] ; Wait for PXCLK to go high (otherwise inputs are "transparent") 22 | 23 | nop side 0x1 [1] ; side set CLK 24 | loop_bit: 25 | in PINS 1 side 0x0 [0] ; Grab a bit of data, clear CLK 26 | jmp x-- loop_bit side 0x1 [1] 27 | 28 | wait 0 pin PIN_OFFS_PXCLK side 0x0 ; Wait for PXCLK to go low 29 | irq set BYTE_IRQ_BASE 30 | .wrap 31 | 32 | % c-sdk { 33 | static inline pio_sm_config camera_pio_get_shift_byte_sm_config(PIO pio, uint sm, uint offset, uint base_pin, uint bpp) 34 | { 35 | pio_sm_config c = camera_pio_shift_byte_program_get_default_config(offset); 36 | sm_config_set_in_pins(&c, base_pin); 37 | sm_config_set_in_shift(&c, true, true, bpp); 38 | sm_config_set_sideset_pins(&c, base_pin + PIN_OFFS_CLK); 39 | 40 | return c; 41 | } 42 | %} 43 | 44 | .program camera_pio_frame 45 | .side_set 1 opt 46 | 47 | .wrap_target 48 | pull ; Pull number of lines 49 | out Y, 32 ; Store number of lines in Y 50 | pull ; Pull "pixels" per line. Keep this in OSR to reload X each line 51 | 52 | wait 1 pin PIN_OFFS_VSYNC ; Wait for start of frame 53 | 54 | loop_line: 55 | mov X, OSR ; Store number of pixels in X 56 | wait 1 pin PIN_OFFS_HREF [2] ; Wait for start of line 57 | 58 | ; The pixel loop body can be patched for different pixel formats / plane 59 | ; layouts. Up to 4 bytes per pixel. 60 | ; For a format with less than 4 bytes per pixel, some of these commands will 61 | ; be patched to NOPs. For multi-plane formats, other SMs will be triggered 62 | ; as appropriate 63 | ; By default, this is set up for 2-bytes per pixel, in a single plane, 64 | ; handled by SM1, transferring "chunks" of 4 bytes (2 pixels) 65 | public loop_pixel: 66 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 0) 67 | wait 1 irq BYTE_IRQ_BASE 68 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 1) 69 | wait 1 irq BYTE_IRQ_BASE 70 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 2) 71 | wait 1 irq BYTE_IRQ_BASE 72 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 3) 73 | wait 1 irq BYTE_IRQ_BASE 74 | jmp x-- loop_pixel 75 | 76 | wait 0 pin PIN_OFFS_HREF ; Wait for end of line 77 | jmp y-- loop_line 78 | 79 | irq wait 0 ; Signal the CPU that we're done, wait for ack 80 | .wrap 81 | 82 | % c-sdk { 83 | static inline void camera_pio_init_gpios(PIO pio, uint sm, uint base_pin) 84 | { 85 | pio_sm_set_consecutive_pindirs(pio, sm, base_pin, 5, false); 86 | pio_sm_set_consecutive_pindirs(pio, sm, base_pin + PIN_OFFS_CLK, 1, true); 87 | 88 | for (uint i = 0; i < 5; i++) { 89 | pio_gpio_init(pio, i + base_pin); 90 | } 91 | 92 | pio->input_sync_bypass = (1 << (base_pin + PIN_OFFS_D0)); 93 | } 94 | 95 | static inline pio_sm_config camera_pio_get_frame_sm_config(PIO pio, uint sm, uint offset, uint base_pin) 96 | { 97 | pio_sm_config c = camera_pio_frame_program_get_default_config(offset); 98 | sm_config_set_in_pins(&c, base_pin); 99 | sm_config_set_out_shift(&c, true, false, 0); 100 | sm_config_set_sideset_pins(&c, base_pin + PIN_OFFS_CLK); 101 | 102 | return c; 103 | } 104 | 105 | static inline bool camera_pio_frame_done(PIO pio) 106 | { 107 | return pio_interrupt_get(pio, 0); 108 | } 109 | 110 | static inline void camera_pio_wait_for_frame_done(PIO pio) 111 | { 112 | while (!camera_pio_frame_done(pio)); 113 | } 114 | 115 | static inline void camera_pio_trigger_frame(PIO pio, uint32_t cols, uint32_t rows) 116 | { 117 | pio_interrupt_clear(pio, 0); 118 | pio_sm_put_blocking(pio, 0, rows - 1); 119 | pio_sm_put_blocking(pio, 0, cols - 1); 120 | } 121 | %} 122 | 123 | .program pixel_loop_yuyv 124 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 0) 125 | wait 1 irq BYTE_IRQ_BASE 126 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 1) 127 | wait 1 irq BYTE_IRQ_BASE 128 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 2) 129 | wait 1 irq BYTE_IRQ_BASE 130 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 3) 131 | wait 1 irq BYTE_IRQ_BASE 132 | 133 | .program pixel_loop_rgb565 134 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 0) 135 | wait 1 irq BYTE_IRQ_BASE 136 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 1) 137 | wait 1 irq BYTE_IRQ_BASE 138 | nop 139 | nop 140 | nop 141 | nop 142 | 143 | // Two-plane YUV: [YY, UV] 144 | .program pixel_loop_nv16 145 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 0) 146 | wait 1 irq BYTE_IRQ_BASE 147 | irq wait (BYTE_IRQ_BASE + 2) ; Trigger byte SM2 (byte 1) 148 | wait 1 irq BYTE_IRQ_BASE 149 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 2) 150 | wait 1 irq BYTE_IRQ_BASE 151 | irq wait (BYTE_IRQ_BASE + 2) ; Trigger byte SM2 (byte 3) 152 | wait 1 irq BYTE_IRQ_BASE 153 | 154 | // Three-plane YUV: [YY, U, V] 155 | .program pixel_loop_yu16 156 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 0) 157 | wait 1 irq BYTE_IRQ_BASE 158 | irq wait (BYTE_IRQ_BASE + 2) ; Trigger byte SM2 (byte 1) 159 | wait 1 irq BYTE_IRQ_BASE 160 | irq wait (BYTE_IRQ_BASE + 1) ; Trigger byte SM1 (byte 2) 161 | wait 1 irq BYTE_IRQ_BASE 162 | irq wait (BYTE_IRQ_BASE + 3) ; Trigger byte SM3 (byte 3) 163 | wait 1 irq BYTE_IRQ_BASE 164 | 165 | % c-sdk { 166 | static inline void camera_pio_patch_pixel_loop(PIO pio, uint offset, const pio_program_t *loop) { 167 | uint i; 168 | // TODO: Assert that length of program is 8? 169 | for (i = 0; i < loop->length; i++) { 170 | pio->instr_mem[offset + camera_pio_frame_offset_loop_pixel + i] = loop->instructions[i]; 171 | } 172 | } 173 | %} 174 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Raspberry-Pi Pico OV7670 Camera Library 2 | 3 | This library can be used to capture images from an OV7670 camera on the 4 | Raspberry Pi Pico, using PIO and DMA. 5 | 6 | It's design for use with a 74165 shift register, to convert the 8-bit parallel 7 | data to a 2-bit serial interface, but directly connecting the parallel data bus 8 | directly to the Pico would also be possible with a slight modification to 9 | the PIO code. 10 | 11 | The PIO portion uses 2-4 state machines from a single PIO instance, depending 12 | on the number of planes in the pixel format. The PIO program(s) use 26 of the 13 | 32 bytes of PIO program memory. 14 | 15 | ## SM0 16 | 17 | State machine 0 handles the "frame"-level timing. 18 | It receives the number of rows and columns via the FIFO, then waits for VSYNC 19 | (start of frame). Then, for each line it waits for HSYNC (start of line) and 20 | triggers the "pixel" state-machine(s) for each byte of data. 21 | 22 | ## SM1-3 23 | 24 | The other state machines handle each byte of data. They will wait for the pixel 25 | to get latched into the 74165 shift register (based on PXCLK), then shift the 26 | data into the PIO ISR, to be pushed into the output FIFO. 27 | 28 | A different state machine is used for each "plane" of image data. 29 | 30 | A different number of bits gets stored in the output FIFO depending on the pixel 31 | format. For example, for the single-plane YUYV/RGB565 formats, 4 bytes at a time 32 | are transferred (representing 2 pixels). For 3-plane YUV422, on the 'Y' state 33 | machine, each transfer is 16 bits (two 'Y' samples), wheras for the 'U' and 'V' 34 | state machines, a single byte is transferred at a time. 35 | 36 | ## DMA 37 | 38 | The DMA is configured to read from each state machine's input FIFO and transfer 39 | the data to the destination buffer. 40 | 41 | ## OV7670 Driver 42 | 43 | The OV7670 sensor is quite poorly documented - there are datasheets out there 44 | and an integration guide, but it leaves out lots of details and there's a huge 45 | number of undocumented (or even reserved) registers which need to be configured 46 | to get a picture out of the camera. 47 | 48 | This library is using Adafruit's OV7670 driver from https://github.com/adafruit/Adafruit_OV7670.git 49 | 50 | I'm aware that there's actually a Pico implementation in that repo, but I had 51 | already written most of my code before I found it. Also, the "arch" they have 52 | doesn't quite fit with how I want to integrate the code. 53 | 54 | So, I've just taken the low-level bits (largely, the reference register settings), 55 | and hacked it up/wrapped it with my camera code. 56 | 57 | This could definitely be done better, but I'm in a hurry and this is working :-). 58 | 59 | ## Circuit 60 | 61 | The code in this repo assumes a specific wiring. 62 | 63 | The `base_pin` and `xclk_pin` can be specified in `camera_platform_config`, 64 | other pins are relative to `base_pin`. 65 | 66 | `xclk_pin` must be one of the `GPOUT` clock sources (21, 23, 24, 25). 67 | 68 | The i2c bus can be connected however you like - just pass in appropriate 69 | functions in `camera_platform_config` (it's done this way so that the i2c 70 | bus can be shared amongst different devices). 71 | 72 | `PCLK` is used to latch the shift register, so it's connected to both the 73 | 74165 and the Pico. 74 | 75 | | OV7670 Module | 74165 | Pico | Pico (`example`) | 76 | | ------------- | ------- | -------------- | ---------------- | 77 | | `SIOC` | - | User decision | 1 | 78 | | `SIOD` | - | User decision | 0 | 79 | | `VSYNC` | - | `base_pin` + 2 | 18 | 80 | | `HREF` | - | `base_pin` + 3 | 19 | 81 | | `PCLK` | `SH/nLD` | `base_pin` + 4 | 20 | 82 | | `XCLK` | - | User decision | 21 | 83 | | `D7` | `A` | - | | 84 | | `D6` | `B` | - | | 85 | | `D5` | `C` | - | | 86 | | `D4` | `D` | - | | 87 | | `D3` | `E` | - | | 88 | | `D2` | `F` | - | | 89 | | `D1` | `G` | - | | 90 | | `D0` | `H` | - | | 91 | | - | `CLK` | `base_pin` + 1 | 17 | 92 | | - | `Qh` | `base_pin` | 16 | 93 | | - | `CLK_INH` | `GND` | | 94 | 95 | ## Shortcomings 96 | 97 | I've written this for a very specific use case (my Pi Wars robot), so I haven't 98 | put much effort into making it very flexible. I'm publishing it as a separate 99 | library because I think it could be a good base to work from. 100 | 101 | Many of the shortcomings below could be quite easily fixed/improved. 102 | 103 | * Resolution is fixed to 80x60 104 | * The Adafruit code can support a few different resolutions, it would be easy 105 | to add support for those. Totally arbitrary resolutions would be harder. 106 | * Only includes PIO code for the shift-register implementation 107 | * A new "byte" program which uses a parallel input would be easy to add. I've 108 | tried that setup during my development, but the code changed a lot since 109 | then. 110 | * Fudged timings/delay cycles. 111 | * I've tweaked some delay numbers to make it work on my set-up with a quite 112 | long cable between the camera and the Pico 113 | * My logic analyser isn't fast enough, and my oscilloscope is too dated to 114 | look at the signals and make some better judgement here. 115 | * No control of frame-rate/PLL frequency 116 | * The register settings just set the OV7670 frequency to 1x the XCLK frequency. 117 | * Could use more efficient transfer sizes 118 | * Could always do 4 byte DMA transfers as long as the buffer sizes are 119 | properly aligned to give multiples of 4 samples. 120 | * Pretty lax error checking/handling. 121 | * `camera_term` not implemented. 122 | 123 | Anyway, if you just want 80x60 RGB565, YUYV or YUV422 images, this should work 124 | great :-) 125 | 126 | ## Example 127 | 128 | The `example` directory has a simple example which captures a frame then 129 | writes an ASCII representation of the Luma (Y) plane to `stdio_usb`. The 130 | character map is designed to look best using **light text on a dark background**. 131 | 132 | Here's an example picture of a cat ornament: 133 | 134 | ``` 135 | +++++************************************************+++++++< 136 | +*******************************************************++++< 137 | ********************************************************++++< 138 | *******************************************************+++++< 139 | +************************************************+++++++++++< 140 | ++++++++****************************************************< 141 | *******************=--+*************************************< 142 | ******************=::::-+***********************************< 143 | *****************+:::::::+**********************+++*********< 144 | *****************=.::::..:=*******************+-:::+*****+++< 145 | *****************-..::....:-=+===--==++*****+-:....-+***++++< 146 | ****************+:..::....:::::::::::::-=++-:......:++++++++< 147 | ++**************+:........::.::::::--::::::::......:+**+++++< 148 | +***************=.........:..::::.:---:::::::......:+***++++< 149 | ****************=...........:...:.:---:.:::::......:+***++++< 150 | ****************=........:........:---:.:::........:+****+++< 151 | ****************=................:----::....::.....:+*****++< 152 | ****************=................:-----:.....:::...-*******+< 153 | ****************+................:-----:......::...=*******+< 154 | ****************+.::.............:-----:........:..=********< 155 | ****************+:::.............:----:.........:.:+********< 156 | ****************+::..............:--=-:.........::-*********< 157 | ****************+:...............:-===:..........:-*********< 158 | ****************+::.......:......:-===:.:........:=*********< 159 | ****************+::.......:......:-===-.:.........-*********< 160 | ****************-::.......::....:-==++=::...:.....:+********< 161 | ****************-::........:::::-======-::::::....:+********< 162 | ****************-:.......:::::-===-----==::::.....:+********< 163 | ****************=:....:::----======----===-:.......=********< 164 | ****************=::...::-===========--=====-:......=********< 165 | ****************=::..::======++=============-:.....=********< 166 | ****************+:..::-====++++++===-=======--:...:+********< 167 | *****************-..:--====++++++===-=======--:...-*********< 168 | *****************+:.:-====++==========-=====---:..-*********< 169 | ******************-::-=================--------:.:+*********< 170 | *******************:::-=================-------:.=**********< 171 | *******************+:.:::-------=======-------:::+**********< 172 | *******************=....::----------=-------:::.:-+*********< 173 | *******************:...::-------------------::...::-=*******< 174 | ******************+:...:--------------------::....:::-+*****< 175 | ******************+::::-=======---===-------=-:::.::::-+****< 176 | ******************=::--=========-====----======-:::::::-+***< 177 | ******************+--============---=---=========::::.::-**+< 178 | ******************+===============--=====++===+===:.....:=+=< 179 | ******************+==+============-=====+++++++===-...::::--< 180 | ******************++++++==========-=====+++++++====:::----::< 181 | *********++++*******++++=========-==--===+++++=====-::---==:< 182 | *******++++++++++***++++========--=--==============:::-====-< 183 | *****+++++++++++++**++++=======---=================---======< 184 | *****+++#%*****+++****+++======---=+=============++++++++++=< 185 | ****+++*#*****##++****+++======--=++=-==========++++++++++==< 186 | ****+++********##+****+++++=-===-=+++====--=====+++++==+++==< 187 | ****+++**********#*****+++*+====++++++==--=====+++++====+===< 188 | ****+++****************++++++++++++++++=======+++++=========< 189 | ****+++*****************++++++++++++++++++++++++++==========< 190 | ****+++********++++*****++++++++++=:-++++++++++++===========< 191 | *****++********++++*****++++++++-:-.:=-=++++++++======++====< 192 | *****+++*******++++*****++++++++-.:-:-.:++++++=========+====< 193 | ******++*******++=+******+++++++-:-==-.:=+++================< 194 | ******+++******++==+*****++++++-::=-:--=++=================-< 195 | ``` 196 | 197 | ## License 198 | 199 | My code is BSD-3 Clause to match the Pico SDK. 200 | 201 | Adafruit's OV7670 driver is MIT. 202 | -------------------------------------------------------------------------------- /camera.c: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2021-2022 Brian Starkey 3 | * 4 | * SPDX-License-Identifier: BSD-3-Clause 5 | */ 6 | #include 7 | #include 8 | 9 | #include "pico/stdlib.h" 10 | #include "hardware/clocks.h" 11 | #include "hardware/dma.h" 12 | #include "hardware/irq.h" 13 | #include "hardware/pio.h" 14 | 15 | #include "camera/camera.h" 16 | #include "camera/format.h" 17 | #include "camera/ov7670.h" 18 | 19 | #include "camera.pio.h" 20 | 21 | #define CAMERA_PIO_FRAME_SM 0 22 | 23 | struct camera *volatile irq_ctxs[2]; 24 | 25 | static inline void __camera_isr(struct camera *camera) 26 | { 27 | if (!camera || !camera->pending) { 28 | return; 29 | } 30 | 31 | if (camera->pending_cb) { 32 | camera->pending_cb(camera->pending, camera->cb_data); 33 | } 34 | 35 | camera->pending = NULL; 36 | } 37 | 38 | static void camera_isr_pio0(void) 39 | { 40 | struct camera *camera = irq_ctxs[0]; 41 | __camera_isr(camera); 42 | 43 | pio_interrupt_clear(pio0, 0); 44 | } 45 | 46 | static void camera_isr_pio1(void) 47 | { 48 | struct camera *camera = irq_ctxs[1]; 49 | __camera_isr(camera); 50 | 51 | pio_interrupt_clear(pio1, 0); 52 | } 53 | 54 | static void camera_pio_init(struct camera *camera) 55 | { 56 | struct camera_platform_config *platform = camera->driver_host.platform; 57 | PIO pio = platform->pio; 58 | 59 | hard_assert(pio == pio0 || pio == pio1); 60 | 61 | camera->shift_byte_offset = pio_add_program(pio, &camera_pio_shift_byte_program); 62 | camera->frame_offset = pio_add_program(pio, &camera_pio_frame_program); 63 | for (int i = 0; i < 4; i++) { 64 | camera_pio_init_gpios(pio, i, platform->base_pin); 65 | } 66 | pio->inte0 = (1 << (8 + CAMERA_PIO_FRAME_SM)); 67 | 68 | if (pio == pio0) { 69 | irq_ctxs[0] = camera; 70 | irq_set_exclusive_handler(PIO0_IRQ_0, camera_isr_pio0); 71 | irq_set_enabled(PIO0_IRQ_0, true); 72 | } else if (pio == pio1) { 73 | irq_ctxs[1] = camera; 74 | irq_set_exclusive_handler(PIO1_IRQ_0, camera_isr_pio1); 75 | irq_set_enabled(PIO1_IRQ_0, true); 76 | } 77 | } 78 | 79 | static bool camera_detect(struct camera_platform_config *platform) 80 | { 81 | const uint8_t reg = OV7670_REG_PID; 82 | uint8_t val = 0; 83 | 84 | int tries = 5; 85 | while (tries--) { 86 | int ret = platform->i2c_write_blocking(platform->i2c_handle, OV7670_ADDR, ®, 1); 87 | if (ret != 1) { 88 | sleep_ms(100); 89 | continue; 90 | } 91 | 92 | ret = platform->i2c_read_blocking(platform->i2c_handle, OV7670_ADDR, &val, 1); 93 | if (ret != 1) { 94 | sleep_ms(100); 95 | continue; 96 | } 97 | 98 | if (val == 0x76) { 99 | break; 100 | } 101 | 102 | sleep_ms(100); 103 | } 104 | 105 | return val == 0x76; 106 | } 107 | 108 | int camera_init(struct camera *camera, struct camera_platform_config *params) 109 | { 110 | OV7670_status status; 111 | 112 | *camera = (struct camera){ 0 }; 113 | 114 | clock_gpio_init(params->xclk_pin, CLOCKS_CLK_GPOUT0_CTRL_AUXSRC_VALUE_CLK_SYS, params->xclk_divider); 115 | 116 | camera->driver_host = (OV7670_host){ 117 | .pins = &(OV7670_pins){ 118 | .enable = -1, 119 | .reset = -1, 120 | }, 121 | .platform = params, 122 | }; 123 | 124 | // Some settling time for the clock 125 | sleep_ms(300); 126 | 127 | // Try and check that the camera is present 128 | if (!camera_detect(params)) { 129 | return -1; 130 | } 131 | 132 | // Note: Frame rate is ignored 133 | status = OV7670_begin(&camera->driver_host, OV7670_COLOR_YUV, OV7670_SIZE_DIV8, 0.0); 134 | if (status != OV7670_STATUS_OK) { 135 | return -1; 136 | } 137 | 138 | if (params->base_dma_channel >= 0) { 139 | for (int i = 0; i < CAMERA_MAX_N_PLANES; i++) { 140 | dma_channel_claim(params->base_dma_channel + i); 141 | camera->dma_channels[i] = params->base_dma_channel + i; 142 | } 143 | } else { 144 | for (int i = 0; i < CAMERA_MAX_N_PLANES; i++) { 145 | camera->dma_channels[i] = dma_claim_unused_channel(true); 146 | } 147 | } 148 | 149 | camera_pio_init(camera); 150 | 151 | // TODO: Set an initial frame config here? 152 | // camera_configure(camera, FORMAT_RGB565, CAMERA_WIDTH_DIV8, CAMERA_HEIGHT_DIV8); 153 | 154 | return 0; 155 | } 156 | 157 | void camera_term(struct camera *camera) 158 | { 159 | // TODO :-) 160 | } 161 | 162 | static OV7670_colorspace ov7670_colorspace_from_format(uint32_t format) 163 | { 164 | switch (format) { 165 | case FORMAT_RGB565: 166 | return OV7670_COLOR_RGB; 167 | case FORMAT_YUYV: 168 | /* Fallthrough */ 169 | case FORMAT_YUV422: 170 | return OV7670_COLOR_YUV; 171 | } 172 | 173 | return 0; 174 | } 175 | 176 | static enum dma_channel_transfer_size camera_transfer_size(uint32_t format, uint8_t plane) 177 | { 178 | switch (format) { 179 | case FORMAT_YUYV: 180 | /* Fallthrough */ 181 | case FORMAT_RGB565: 182 | return DMA_SIZE_32; 183 | case FORMAT_YUV422: 184 | return plane ? DMA_SIZE_8 : DMA_SIZE_16; 185 | default: 186 | return 0; 187 | } 188 | } 189 | 190 | static uint8_t __dma_transfer_size_to_bytes(enum dma_channel_transfer_size dma) 191 | { 192 | switch (dma) { 193 | case DMA_SIZE_8: 194 | return 1; 195 | case DMA_SIZE_16: 196 | return 2; 197 | case DMA_SIZE_32: 198 | return 4; 199 | } 200 | 201 | return 0; 202 | } 203 | 204 | static const pio_program_t *camera_get_pixel_loop(uint32_t format) 205 | { 206 | switch (format) { 207 | case FORMAT_YUYV: 208 | return &pixel_loop_yuyv_program; 209 | case FORMAT_RGB565: 210 | // XXX: Should really use a 16-bit loop and swap to 2 bytes 211 | // per chunk instead of 4. 212 | return &pixel_loop_yuyv_program; 213 | case FORMAT_YUV422: 214 | return &pixel_loop_yu16_program; 215 | default: 216 | return NULL; 217 | } 218 | } 219 | 220 | static void camera_pio_configure(struct camera *camera) 221 | { 222 | struct camera_platform_config *platform = camera->driver_host.platform; 223 | 224 | // First disable everything 225 | pio_set_sm_mask_enabled(platform->pio, 0xf, false); 226 | 227 | // Then reset everything 228 | pio_restart_sm_mask(platform->pio, 0xf); 229 | 230 | // Drain the FIFOs 231 | for (int i = 0; i < 4; i++) { 232 | pio_sm_clear_fifos(platform->pio, i); 233 | } 234 | 235 | // Patch the pixel loop 236 | const pio_program_t *pixel_loop = camera_get_pixel_loop(camera->config.format); 237 | camera_pio_patch_pixel_loop(platform->pio, camera->frame_offset, pixel_loop); 238 | 239 | // Finally we can configure and enable the state machines 240 | uint8_t num_planes = format_num_planes(camera->config.format); 241 | for (int i = 0; i < num_planes; i++) { 242 | pio_sm_init(platform->pio, i + 1, camera->shift_byte_offset, &camera->config.sm_cfgs[i + 1]); 243 | pio_sm_set_enabled(platform->pio, i + 1, true); 244 | } 245 | 246 | pio_sm_init(platform->pio, CAMERA_PIO_FRAME_SM, camera->frame_offset, &camera->config.sm_cfgs[CAMERA_PIO_FRAME_SM]); 247 | pio_sm_set_enabled(platform->pio, CAMERA_PIO_FRAME_SM, true); 248 | } 249 | 250 | int camera_configure(struct camera *camera, uint32_t format, uint16_t width, uint16_t height) 251 | { 252 | if (width != CAMERA_WIDTH_DIV8 || height != CAMERA_HEIGHT_DIV8) { 253 | // TODO: Handle other sizes 254 | return -1; 255 | } 256 | 257 | struct camera_platform_config *platform = camera->driver_host.platform; 258 | 259 | OV7670_set_format(platform, ov7670_colorspace_from_format(format)); 260 | OV7670_set_size(platform, OV7670_SIZE_DIV8); 261 | 262 | camera->config.sm_cfgs[CAMERA_PIO_FRAME_SM] = 263 | camera_pio_get_frame_sm_config(platform->pio, CAMERA_PIO_FRAME_SM, camera->frame_offset, platform->base_pin); 264 | 265 | uint8_t num_planes = format_num_planes(format); 266 | for (int i = 0; i < num_planes; i++) { 267 | enum dma_channel_transfer_size xfer_size = camera_transfer_size(format, i); 268 | 269 | dma_channel_config c = dma_channel_get_default_config(camera->dma_channels[i]); 270 | channel_config_set_transfer_data_size(&c, xfer_size); 271 | channel_config_set_read_increment(&c, false); 272 | channel_config_set_write_increment(&c, true); 273 | channel_config_set_dreq(&c, pio_get_dreq(platform->pio, i + 1, false)); 274 | 275 | camera->config.dma_cfgs[i] = c; 276 | 277 | uint8_t xfer_bytes = __dma_transfer_size_to_bytes(xfer_size); 278 | camera->config.dma_offset[i] = 4 - xfer_bytes, 279 | camera->config.dma_transfers[i] = format_plane_size(format, i, width, height) / xfer_bytes, 280 | 281 | camera->config.sm_cfgs[i + 1] = camera_pio_get_shift_byte_sm_config(platform->pio, i + 1, 282 | camera->shift_byte_offset, platform->base_pin, 283 | xfer_bytes * 8); 284 | } 285 | 286 | camera->config.format = format; 287 | camera->config.width = width; 288 | camera->config.height = height; 289 | 290 | camera_pio_configure(camera); 291 | 292 | return 0; 293 | } 294 | 295 | static uint8_t camera_pixels_per_chunk(uint32_t format) 296 | { 297 | switch (format) { 298 | case FORMAT_YUYV: 299 | /* Fallthrough */ 300 | case FORMAT_RGB565: 301 | return 2; 302 | case FORMAT_YUV422: 303 | return 2; 304 | default: 305 | return 1; 306 | } 307 | } 308 | 309 | static int camera_do_frame(struct camera *camera, struct camera_buffer *buf, camera_frame_cb complete_cb, void *cb_data, 310 | bool allow_reconfigure, bool blocking) 311 | { 312 | if (camera->pending) { 313 | return -2; 314 | } 315 | 316 | if ((camera->config.format != buf->format) || 317 | (camera->config.width != buf->width) || 318 | (camera->config.height != buf->height)) { 319 | if (allow_reconfigure) { 320 | camera_configure(camera, buf->format, buf->width, buf->height); 321 | } else { 322 | return -1; 323 | } 324 | } 325 | 326 | struct camera_platform_config *platform = camera->driver_host.platform; 327 | 328 | uint8_t num_planes = format_num_planes(camera->config.format); 329 | for (int i = 0; i < num_planes; i++) { 330 | dma_channel_configure(camera->dma_channels[i], 331 | &camera->config.dma_cfgs[i], 332 | buf->data[i], 333 | ((char *)&platform->pio->rxf[i + 1]) + camera->config.dma_offset[i], 334 | camera->config.dma_transfers[i], 335 | true); 336 | } 337 | 338 | uint32_t num_loops = buf->width / camera_pixels_per_chunk(buf->format); 339 | 340 | camera->pending = buf; 341 | camera->pending_cb = complete_cb; 342 | camera->cb_data = cb_data; 343 | 344 | camera_pio_trigger_frame(platform->pio, num_loops, buf->height); 345 | 346 | if (blocking) { 347 | while (camera->pending) { 348 | sleep_ms(1); 349 | } 350 | } 351 | 352 | return 0; 353 | } 354 | 355 | int camera_capture_blocking(struct camera *camera, struct camera_buffer *into, bool allow_reconfigure) 356 | { 357 | return camera_do_frame(camera, into, NULL, NULL, allow_reconfigure, true); 358 | } 359 | 360 | int camera_capture_with_cb(struct camera *camera, struct camera_buffer *into, bool allow_reconfigure, 361 | camera_frame_cb complete_cb, void *cb_data) 362 | { 363 | return camera_do_frame(camera, into, complete_cb, cb_data, allow_reconfigure, false); 364 | } 365 | 366 | struct camera_buffer *camera_buffer_alloc(uint32_t format, uint16_t width, uint16_t height) 367 | { 368 | struct camera_buffer *buf = malloc(sizeof(*buf)); 369 | if (!buf) { 370 | return NULL; 371 | } 372 | 373 | *buf = (struct camera_buffer){ 374 | .format = format, 375 | .width = width, 376 | .height = height, 377 | }; 378 | 379 | uint8_t num_planes = format_num_planes(format); 380 | for (int i = 0; i < num_planes; i++) { 381 | buf->strides[i] = format_stride(format, i, width); 382 | buf->sizes[i] = format_plane_size(format, i, width, height); 383 | buf->data[i] = malloc(buf->sizes[i]); 384 | if (!buf->data[i]) { 385 | goto error; 386 | } 387 | } 388 | 389 | return buf; 390 | 391 | error: 392 | for (int i = 0; i < num_planes; i++) { 393 | if (buf->data[i]) { 394 | free(buf->data[i]); 395 | } 396 | } 397 | free(buf); 398 | return NULL; 399 | } 400 | 401 | void camera_buffer_free(struct camera_buffer *buf) 402 | { 403 | if (buf == NULL) { 404 | return; 405 | } 406 | 407 | uint8_t num_planes = format_num_planes(buf->format); 408 | for (int i = 0; i < num_planes; i++) { 409 | if (buf->data[i]) { 410 | free(buf->data[i]); 411 | } 412 | } 413 | free(buf); 414 | } 415 | 416 | // Adafruit driver functions 417 | 418 | void OV7670_print(char *str) 419 | { 420 | // TODO 421 | } 422 | 423 | int OV7670_read_register(void *platform, uint8_t reg) 424 | { 425 | struct camera_platform_config *pcfg = (struct camera_platform_config *)platform; 426 | uint8_t value; 427 | 428 | pcfg->i2c_write_blocking(pcfg->i2c_handle, OV7670_ADDR, ®, 1); 429 | pcfg->i2c_read_blocking(pcfg->i2c_handle, OV7670_ADDR, &value, 1); 430 | 431 | return value; 432 | } 433 | 434 | void OV7670_write_register(void *platform, uint8_t reg, uint8_t value) 435 | { 436 | struct camera_platform_config *pcfg = (struct camera_platform_config *)platform; 437 | 438 | pcfg->i2c_write_blocking(pcfg->i2c_handle, OV7670_ADDR, (uint8_t[]){ reg, value }, 2); 439 | } 440 | -------------------------------------------------------------------------------- /include/camera/ov7670.h: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2020 P Burgess for Adafruit Industries 2 | // 3 | // SPDX-License-Identifier: MIT 4 | 5 | #pragma once 6 | 7 | #include "pico/stdlib.h" 8 | 9 | // IMPORTANT: #include ALL of the arch-specific .h files here. 10 | // They have #ifdef checks to only take effect on the active architecture. 11 | //#include "arch/samd51.h" 12 | 13 | #if defined(ARDUINO) 14 | #include 15 | #define OV7670_delay_ms(x) delay(x) 16 | #define OV7670_pin_output(pin) pinMode(pin, OUTPUT); 17 | #define OV7670_pin_write(pin, hi) digitalWrite(pin, hi ? 1 : 0) 18 | #define OV7670_disable_interrupts() noInterrupts() 19 | #define OV7670_enable_interrupts() interrupts() 20 | #else 21 | #include 22 | // If platform provides device-agnostic functions for millisecond delay, 23 | // set-pin-to-output, pin-write and/or interrupts on/off, those can be 24 | // #defined here as with Arduino above. If platform does NOT provide some 25 | // or all of these, they should go in the device-specific .c file with a 26 | // !defined(ARDUINO) around them. 27 | #define OV7670_delay_ms(x) sleep_ms(x) 28 | #define OV7670_pin_output(pin) { gpio_init(pin); gpio_set_dir(pin, GPIO_OUT); } 29 | #define OV7670_pin_write(pin, hi) gpio_put(pin, !!hi) 30 | #endif // end platforms 31 | 32 | #define OV7670_XCLK_HZ 15625000 33 | 34 | typedef int OV7670_pin; 35 | typedef int OV7670_arch; 36 | 37 | /** Status codes returned by some functions */ 38 | typedef enum { 39 | OV7670_STATUS_OK = 0, ///< Success 40 | OV7670_STATUS_ERR_MALLOC, ///< malloc() call failed 41 | OV7670_STATUS_ERR_PERIPHERAL, ///< Peripheral (e.g. timer) not found 42 | } OV7670_status; 43 | 44 | /** Supported color formats */ 45 | typedef enum { 46 | OV7670_COLOR_RGB = 0, ///< RGB565 big-endian 47 | OV7670_COLOR_YUV, ///< YUV/YCbCr 4:2:2 big-endian 48 | } OV7670_colorspace; 49 | 50 | /** Supported sizes (VGA division factor) for OV7670_set_size() */ 51 | typedef enum { 52 | OV7670_SIZE_DIV1 = 0, ///< 640 x 480 53 | OV7670_SIZE_DIV2, ///< 320 x 240 54 | OV7670_SIZE_DIV4, ///< 160 x 120 55 | OV7670_SIZE_DIV8, ///< 80 x 60 56 | OV7670_SIZE_DIV16, ///< 40 x 30 57 | } OV7670_size; 58 | 59 | typedef enum { 60 | OV7670_TEST_PATTERN_NONE = 0, ///< Disable test pattern 61 | OV7670_TEST_PATTERN_SHIFTING_1, ///< "Shifting 1" pattern 62 | OV7670_TEST_PATTERN_COLOR_BAR, ///< 8 color bars 63 | OV7670_TEST_PATTERN_COLOR_BAR_FADE, ///< Color bars w/fade to white 64 | } OV7670_pattern; 65 | 66 | typedef enum { 67 | OV7670_NIGHT_MODE_OFF = 0, ///< Disable night mode 68 | OV7670_NIGHT_MODE_2, ///< Night mode 1/2 frame rate 69 | OV7670_NIGHT_MODE_4, ///< Night mode 1/4 frame rate 70 | OV7670_NIGHT_MODE_8, ///< Night mode 1/8 frame rate 71 | } OV7670_night_mode; 72 | 73 | /** 74 | Defines physical connection to OV7670 camera, passed to constructor. 75 | On certain architectures, some of these pins are fixed in hardware and 76 | cannot be changed, e.g. SAMD51 has its Parallel Capture Controller in 77 | one specific place. In such cases, you can usually avoid declaring those 78 | values when setting up the structure. However, elements DO need to be 79 | declared in-order matching the structure and without gaps, and struct 80 | elements have been sorted with this in mind. You can cut off a declaration 81 | early, but if middle elements aren't needed must still be assigned some 82 | unused value (e.g. 0). On Arduino platform, SDA/SCL are dictated by the 83 | Wire instance and don't need to be set here. See example code. 84 | */ 85 | typedef struct { 86 | OV7670_pin enable; ///< Also called PWDN, or set to -1 and tie to GND 87 | OV7670_pin reset; ///< Cam reset, or set to -1 and tie to 3.3V 88 | OV7670_pin xclk; ///< MCU clock out / cam clock in 89 | OV7670_pin pclk; ///< Cam clock out / MCU clock in 90 | OV7670_pin vsync; ///< Also called DEN1 91 | OV7670_pin hsync; ///< Also called DEN2 92 | OV7670_pin data[8]; ///< Camera parallel data out 93 | OV7670_pin sda; ///< I2C data 94 | OV7670_pin scl; ///< I2C clock 95 | } OV7670_pins; 96 | 97 | /** Address/value combo for OV7670 camera commands. */ 98 | typedef struct { 99 | uint8_t reg; ///< Register address 100 | uint8_t value; ///< Value to store 101 | } OV7670_command; 102 | 103 | /** Architecture+platform combination structure. */ 104 | typedef struct { 105 | OV7670_arch *arch; ///< Architecture-specific config data 106 | OV7670_pins *pins; ///< Physical connection to camera 107 | void *platform; ///< Platform-specific data (e.g. Arduino C++ object) 108 | } OV7670_host; 109 | 110 | #define OV7670_ADDR 0x21 //< Default I2C address if unspecified 111 | 112 | #define OV7670_REG_GAIN 0x00 //< AGC gain bits 7:0 (9:8 in VREF) 113 | #define OV7670_REG_BLUE 0x01 //< AWB blue channel gain 114 | #define OV7670_REG_RED 0x02 //< AWB red channel gain 115 | #define OV7670_REG_VREF 0x03 //< Vert frame control bits 116 | #define OV7670_REG_COM1 0x04 //< Common control 1 117 | #define OV7670_COM1_R656 0x40 //< COM1 enable R656 format 118 | #define OV7670_REG_BAVE 0x05 //< U/B average level 119 | #define OV7670_REG_GbAVE 0x06 //< Y/Gb average level 120 | #define OV7670_REG_AECHH 0x07 //< Exposure value - AEC 15:10 bits 121 | #define OV7670_REG_RAVE 0x08 //< V/R average level 122 | #define OV7670_REG_COM2 0x09 //< Common control 2 123 | #define OV7670_COM2_SSLEEP 0x10 //< COM2 soft sleep mode 124 | #define OV7670_REG_PID 0x0A //< Product ID MSB (read-only) 125 | #define OV7670_REG_VER 0x0B //< Product ID LSB (read-only) 126 | #define OV7670_REG_COM3 0x0C //< Common control 3 127 | #define OV7670_COM3_SWAP 0x40 //< COM3 output data MSB/LSB swap 128 | #define OV7670_COM3_SCALEEN 0x08 //< COM3 scale enable 129 | #define OV7670_COM3_DCWEN 0x04 //< COM3 DCW enable 130 | #define OV7670_REG_COM4 0x0D //< Common control 4 131 | #define OV7670_REG_COM5 0x0E //< Common control 5 132 | #define OV7670_REG_COM6 0x0F //< Common control 6 133 | #define OV7670_REG_AECH 0x10 //< Exposure value 9:2 134 | #define OV7670_REG_CLKRC 0x11 //< Internal clock 135 | #define OV7670_CLK_EXT 0x40 //< CLKRC Use ext clock directly 136 | #define OV7670_CLK_SCALE 0x3F //< CLKRC Int clock prescale mask 137 | #define OV7670_REG_COM7 0x12 //< Common control 7 138 | #define OV7670_COM7_RESET 0x80 //< COM7 SCCB register reset 139 | #define OV7670_COM7_SIZE_MASK 0x38 //< COM7 output size mask 140 | #define OV7670_COM7_PIXEL_MASK 0x05 //< COM7 output pixel format mask 141 | #define OV7670_COM7_SIZE_VGA 0x00 //< COM7 output size VGA 142 | #define OV7670_COM7_SIZE_CIF 0x20 //< COM7 output size CIF 143 | #define OV7670_COM7_SIZE_QVGA 0x10 //< COM7 output size QVGA 144 | #define OV7670_COM7_SIZE_QCIF 0x08 //< COM7 output size QCIF 145 | #define OV7670_COM7_RGB 0x04 //< COM7 pixel format RGB 146 | #define OV7670_COM7_YUV 0x00 //< COM7 pixel format YUV 147 | #define OV7670_COM7_BAYER 0x01 //< COM7 pixel format Bayer RAW 148 | #define OV7670_COM7_PBAYER 0x05 //< COM7 pixel fmt proc Bayer RAW 149 | #define OV7670_COM7_COLORBAR 0x02 //< COM7 color bar enable 150 | #define OV7670_REG_COM8 0x13 //< Common control 8 151 | #define OV7670_COM8_FASTAEC 0x80 //< COM8 Enable fast AGC/AEC algo, 152 | #define OV7670_COM8_AECSTEP 0x40 //< COM8 AEC step size unlimited 153 | #define OV7670_COM8_BANDING 0x20 //< COM8 Banding filter enable 154 | #define OV7670_COM8_AGC 0x04 //< COM8 AGC (auto gain) enable 155 | #define OV7670_COM8_AWB 0x02 //< COM8 AWB (auto white balance) 156 | #define OV7670_COM8_AEC 0x01 //< COM8 AEC (auto exposure) enable 157 | #define OV7670_REG_COM9 0x14 //< Common control 9 - max AGC value 158 | #define OV7670_REG_COM10 0x15 //< Common control 10 159 | #define OV7670_COM10_HSYNC 0x40 //< COM10 HREF changes to HSYNC 160 | #define OV7670_COM10_PCLK_HB 0x20 //< COM10 Suppress PCLK on hblank 161 | #define OV7670_COM10_HREF_REV 0x08 //< COM10 HREF reverse 162 | #define OV7670_COM10_VS_EDGE 0x04 //< COM10 VSYNC chg on PCLK rising 163 | #define OV7670_COM10_VS_NEG 0x02 //< COM10 VSYNC negative 164 | #define OV7670_COM10_HS_NEG 0x01 //< COM10 HSYNC negative 165 | #define OV7670_REG_HSTART 0x17 //< Horiz frame start high bits 166 | #define OV7670_REG_HSTOP 0x18 //< Horiz frame end high bits 167 | #define OV7670_REG_VSTART 0x19 //< Vert frame start high bits 168 | #define OV7670_REG_VSTOP 0x1A //< Vert frame end high bits 169 | #define OV7670_REG_PSHFT 0x1B //< Pixel delay select 170 | #define OV7670_REG_MIDH 0x1C //< Manufacturer ID high byte 171 | #define OV7670_REG_MIDL 0x1D //< Manufacturer ID low byte 172 | #define OV7670_REG_MVFP 0x1E //< Mirror / vert-flip enable 173 | #define OV7670_MVFP_MIRROR 0x20 //< MVFP Mirror image 174 | #define OV7670_MVFP_VFLIP 0x10 //< MVFP Vertical flip 175 | #define OV7670_REG_LAEC 0x1F //< Reserved 176 | #define OV7670_REG_ADCCTR0 0x20 //< ADC control 177 | #define OV7670_REG_ADCCTR1 0x21 //< Reserved 178 | #define OV7670_REG_ADCCTR2 0x22 //< Reserved 179 | #define OV7670_REG_ADCCTR3 0x23 //< Reserved 180 | #define OV7670_REG_AEW 0x24 //< AGC/AEC upper limit 181 | #define OV7670_REG_AEB 0x25 //< AGC/AEC lower limit 182 | #define OV7670_REG_VPT 0x26 //< AGC/AEC fast mode op region 183 | #define OV7670_REG_BBIAS 0x27 //< B channel signal output bias 184 | #define OV7670_REG_GbBIAS 0x28 //< Gb channel signal output bias 185 | #define OV7670_REG_EXHCH 0x2A //< Dummy pixel insert MSB 186 | #define OV7670_REG_EXHCL 0x2B //< Dummy pixel insert LSB 187 | #define OV7670_REG_RBIAS 0x2C //< R channel signal output bias 188 | #define OV7670_REG_ADVFL 0x2D //< Insert dummy lines MSB 189 | #define OV7670_REG_ADVFH 0x2E //< Insert dummy lines LSB 190 | #define OV7670_REG_YAVE 0x2F //< Y/G channel average value 191 | #define OV7670_REG_HSYST 0x30 //< HSYNC rising edge delay 192 | #define OV7670_REG_HSYEN 0x31 //< HSYNC falling edge delay 193 | #define OV7670_REG_HREF 0x32 //< HREF control 194 | #define OV7670_REG_CHLF 0x33 //< Array current control 195 | #define OV7670_REG_ARBLM 0x34 //< Array ref control - reserved 196 | #define OV7670_REG_ADC 0x37 //< ADC control - reserved 197 | #define OV7670_REG_ACOM 0x38 //< ADC & analog common - reserved 198 | #define OV7670_REG_OFON 0x39 //< ADC offset control - reserved 199 | #define OV7670_REG_TSLB 0x3A //< Line buffer test option 200 | #define OV7670_TSLB_NEG 0x20 //< TSLB Negative image enable 201 | #define OV7670_TSLB_YLAST 0x04 //< TSLB UYVY or VYUY, see COM13 202 | #define OV7670_TSLB_AOW 0x01 //< TSLB Auto output window 203 | #define OV7670_REG_COM11 0x3B //< Common control 11 204 | #define OV7670_COM11_NIGHT 0x80 //< COM11 Night mode 205 | #define OV7670_COM11_NMFR 0x60 //< COM11 Night mode frame rate mask 206 | #define OV7670_COM11_HZAUTO 0x10 //< COM11 Auto detect 50/60 Hz 207 | #define OV7670_COM11_BAND 0x08 //< COM11 Banding filter val select 208 | #define OV7670_COM11_EXP 0x02 //< COM11 Exposure timing control 209 | #define OV7670_REG_COM12 0x3C //< Common control 12 210 | #define OV7670_COM12_HREF 0x80 //< COM12 Always has HREF 211 | #define OV7670_REG_COM13 0x3D //< Common control 13 212 | #define OV7670_COM13_GAMMA 0x80 //< COM13 Gamma enable 213 | #define OV7670_COM13_UVSAT 0x40 //< COM13 UV saturation auto adj 214 | #define OV7670_COM13_UVSWAP 0x01 //< COM13 UV swap, use w TSLB[3] 215 | #define OV7670_REG_COM14 0x3E //< Common control 14 216 | #define OV7670_COM14_DCWEN 0x10 //< COM14 DCW & scaling PCLK enable 217 | #define OV7670_REG_EDGE 0x3F //< Edge enhancement adjustment 218 | #define OV7670_REG_COM15 0x40 //< Common control 15 219 | #define OV7670_COM15_RMASK 0xC0 //< COM15 Output range mask 220 | #define OV7670_COM15_R10F0 0x00 //< COM15 Output range 10 to F0 221 | #define OV7670_COM15_R01FE 0x80 //< COM15 01 to FE 222 | #define OV7670_COM15_R00FF 0xC0 //< COM15 00 to FF 223 | #define OV7670_COM15_RGBMASK 0x30 //< COM15 RGB 555/565 option mask 224 | #define OV7670_COM15_RGB 0x00 //< COM15 Normal RGB out 225 | #define OV7670_COM15_RGB565 0x10 //< COM15 RGB 565 output 226 | #define OV7670_COM15_RGB555 0x30 //< COM15 RGB 555 output 227 | #define OV7670_REG_COM16 0x41 //< Common control 16 228 | #define OV7670_COM16_AWBGAIN 0x08 //< COM16 AWB gain enable 229 | #define OV7670_REG_COM17 0x42 //< Common control 17 230 | #define OV7670_COM17_AECWIN 0xC0 //< COM17 AEC window must match COM4 231 | #define OV7670_COM17_CBAR 0x08 //< COM17 DSP Color bar enable 232 | #define OV7670_REG_AWBC1 0x43 //< Reserved 233 | #define OV7670_REG_AWBC2 0x44 //< Reserved 234 | #define OV7670_REG_AWBC3 0x45 //< Reserved 235 | #define OV7670_REG_AWBC4 0x46 //< Reserved 236 | #define OV7670_REG_AWBC5 0x47 //< Reserved 237 | #define OV7670_REG_AWBC6 0x48 //< Reserved 238 | #define OV7670_REG_REG4B 0x4B //< UV average enable 239 | #define OV7670_REG_DNSTH 0x4C //< De-noise strength 240 | #define OV7670_REG_MTX1 0x4F //< Matrix coefficient 1 241 | #define OV7670_REG_MTX2 0x50 //< Matrix coefficient 2 242 | #define OV7670_REG_MTX3 0x51 //< Matrix coefficient 3 243 | #define OV7670_REG_MTX4 0x52 //< Matrix coefficient 4 244 | #define OV7670_REG_MTX5 0x53 //< Matrix coefficient 5 245 | #define OV7670_REG_MTX6 0x54 //< Matrix coefficient 6 246 | #define OV7670_REG_BRIGHT 0x55 //< Brightness control 247 | #define OV7670_REG_CONTRAS 0x56 //< Contrast control 248 | #define OV7670_REG_CONTRAS_CENTER 0x57 //< Contrast center 249 | #define OV7670_REG_MTXS 0x58 //< Matrix coefficient sign 250 | #define OV7670_REG_LCC1 0x62 //< Lens correction option 1 251 | #define OV7670_REG_LCC2 0x63 //< Lens correction option 2 252 | #define OV7670_REG_LCC3 0x64 //< Lens correction option 3 253 | #define OV7670_REG_LCC4 0x65 //< Lens correction option 4 254 | #define OV7670_REG_LCC5 0x66 //< Lens correction option 5 255 | #define OV7670_REG_MANU 0x67 //< Manual U value 256 | #define OV7670_REG_MANV 0x68 //< Manual V value 257 | #define OV7670_REG_GFIX 0x69 //< Fix gain control 258 | #define OV7670_REG_GGAIN 0x6A //< G channel AWB gain 259 | #define OV7670_REG_DBLV 0x6B //< PLL & regulator control 260 | #define OV7670_REG_AWBCTR3 0x6C //< AWB control 3 261 | #define OV7670_REG_AWBCTR2 0x6D //< AWB control 2 262 | #define OV7670_REG_AWBCTR1 0x6E //< AWB control 1 263 | #define OV7670_REG_AWBCTR0 0x6F //< AWB control 0 264 | #define OV7670_REG_SCALING_XSC 0x70 //< Test pattern X scaling 265 | #define OV7670_REG_SCALING_YSC 0x71 //< Test pattern Y scaling 266 | #define OV7670_REG_SCALING_DCWCTR 0x72 //< DCW control 267 | #define OV7670_REG_SCALING_PCLK_DIV 0x73 //< DSP scale control clock divide 268 | #define OV7670_REG_REG74 0x74 //< Digital gain control 269 | #define OV7670_REG_REG76 0x76 //< Pixel correction 270 | #define OV7670_REG_SLOP 0x7A //< Gamma curve highest seg slope 271 | #define OV7670_REG_GAM_BASE 0x7B //< Gamma register base (1 of 15) 272 | #define OV7670_GAM_LEN 15 //< Number of gamma registers 273 | #define OV7670_R76_BLKPCOR 0x80 //< REG76 black pixel corr enable 274 | #define OV7670_R76_WHTPCOR 0x40 //< REG76 white pixel corr enable 275 | #define OV7670_REG_RGB444 0x8C //< RGB 444 control 276 | #define OV7670_R444_ENABLE 0x02 //< RGB444 enable 277 | #define OV7670_R444_RGBX 0x01 //< RGB444 word format 278 | #define OV7670_REG_DM_LNL 0x92 //< Dummy line LSB 279 | #define OV7670_REG_LCC6 0x94 //< Lens correction option 6 280 | #define OV7670_REG_LCC7 0x95 //< Lens correction option 7 281 | #define OV7670_REG_HAECC1 0x9F //< Histogram-based AEC/AGC ctrl 1 282 | #define OV7670_REG_HAECC2 0xA0 //< Histogram-based AEC/AGC ctrl 2 283 | #define OV7670_REG_SCALING_PCLK_DELAY 0xA2 //< Scaling pixel clock delay 284 | #define OV7670_REG_BD50MAX 0xA5 //< 50 Hz banding step limit 285 | #define OV7670_REG_HAECC3 0xA6 //< Histogram-based AEC/AGC ctrl 3 286 | #define OV7670_REG_HAECC4 0xA7 //< Histogram-based AEC/AGC ctrl 4 287 | #define OV7670_REG_HAECC5 0xA8 //< Histogram-based AEC/AGC ctrl 5 288 | #define OV7670_REG_HAECC6 0xA9 //< Histogram-based AEC/AGC ctrl 6 289 | #define OV7670_REG_HAECC7 0xAA //< Histogram-based AEC/AGC ctrl 7 290 | #define OV7670_REG_BD60MAX 0xAB //< 60 Hz banding step limit 291 | #define OV7670_REG_ABLC1 0xB1 //< ABLC enable 292 | #define OV7670_REG_THL_ST 0xB3 //< ABLC target 293 | #define OV7670_REG_SATCTR 0xC9 //< Saturation control 294 | 295 | #define OV7670_REG_LAST OV7670_REG_SATCTR //< Maximum register address 296 | 297 | // C++ ACCESSIBLE FUNCTIONS ------------------------------------------------ 298 | 299 | // These are declared in an extern "C" so Arduino platform C++ code can 300 | // access them. 301 | 302 | #ifdef __cplusplus 303 | extern "C" { 304 | #endif 305 | 306 | // Architecture- and platform-neutral initialization function. 307 | // Called by the platform init function, this in turn may call an 308 | // architecture-specific init function. 309 | OV7670_status OV7670_begin(OV7670_host *host, OV7670_colorspace colorspace, 310 | OV7670_size size, float fps); 311 | 312 | OV7670_status OV7670_set_format(void *platform, OV7670_colorspace colorspace); 313 | 314 | // Configure camera frame rate. Actual resulting frame rate (returned) may 315 | // be different depending on available clock frequencies. Result will only 316 | // exceed input if necessary for minimum supported rate, but this is very 317 | // rare, typically below 1 fps. In all other cases, result will be equal 318 | // or less than the requested rate, up to a maximum of 30 fps (the "or less" 319 | // is because requested fps may be based on other host hardware timing 320 | // constraints (e.g. screen) and rounding up to a closer-but-higher frame 321 | // rate would be problematic). There is no hardcoded set of fixed frame 322 | // rates because it varies with architecture, depending on OV7670_XCLK_HZ. 323 | float OV7670_set_fps(void *platform, float fps); 324 | 325 | // Configure camera resolution to one of the supported frame sizes 326 | // (powers-of-two divisions of VGA -- 640x480 down to 40x30). 327 | void OV7670_set_size(void *platform, OV7670_size size); 328 | 329 | // Lower-level resolution register fiddling function, exposed so dev code 330 | // can test variations for OV7670_set_size() windowing defaults. 331 | void OV7670_frame_control(void *platform, uint8_t size, uint8_t vstart, 332 | uint16_t hstart, uint8_t edge_offset, 333 | uint8_t pclk_delay); 334 | 335 | // Select one of the camera's night modes (or disable). 336 | // Trades off frame rate for less grainy images in low light. 337 | void OV7670_night(void *platform, OV7670_night_mode night); 338 | 339 | // Flips camera output on horizontal and/or vertical axes. 340 | void OV7670_flip(void *platform, bool flip_x, bool flip_y); 341 | 342 | // Selects one of the camera's test patterns (or disable). 343 | // See Adafruit_OV7670.h for notes about minor visual bug here. 344 | void OV7670_test_pattern(void *platform, OV7670_pattern pattern); 345 | 346 | // Convert Y (brightness) component YUV image in RAM to RGB565 big- 347 | // endian format for preview on TFT display. Data is overwritten in-place, 348 | // Y is truncated and UV elements are lost. No practical use outside TFT 349 | // preview. If you need actual grayscale 0-255 data, just access the low 350 | // byte of each 16-bit YUV pixel. 351 | void OV7670_Y2RGB565(uint16_t *ptr, uint32_t len); 352 | 353 | #ifdef __cplusplus 354 | }; 355 | #endif 356 | -------------------------------------------------------------------------------- /ov7670.c: -------------------------------------------------------------------------------- 1 | // SPDX-FileCopyrightText: 2020 P Burgess for Adafruit Industries 2 | // 3 | // SPDX-License-Identifier: MIT 4 | 5 | #include "camera/ov7670.h" 6 | 7 | // REQUIRED EXTERN FUNCTIONS ----------------------------------------------- 8 | 9 | // Outside code MUST provide these functions with the same arguments and 10 | // return types (see end of Adafruit_OV7670.cpp for an example). These allow 11 | // basic debug-print and I2C operations without referencing C++ objects or 12 | // accessing specific peripherals from this code. That information is all 13 | // contained in the higher-level calling code, which can set up the platform 14 | // pointer for whatever objects and/or peripherals it needs. Yes this is 15 | // ugly and roundabout, but it makes this part of the code more reusable 16 | // (not having to re-implement for each platform/architecture) and the 17 | // functions in question aren't performance-oriented anyway (typically used 18 | // just once on startup, or during I2C transfers which are slow anyway). 19 | 20 | extern void OV7670_print(char *str); 21 | extern int OV7670_read_register(void *platform, uint8_t reg); 22 | extern void OV7670_write_register(void *platform, uint8_t reg, uint8_t value); 23 | 24 | // UTILITY FUNCTIONS ------------------------------------------------------- 25 | 26 | // Write a 0xFF-terminated list of commands to the camera. 27 | // First argument is a pointer to platform-specific data...e.g. on Arduno, 28 | // this points to a C++ object so we can find our way back to the correct 29 | // I2C peripheral (so this code doesn't have to deal with platform-specific 30 | // I2C calls). 31 | void OV7670_write_list(void *platform, OV7670_command *cmd) { 32 | for (int i = 0; cmd[i].reg <= OV7670_REG_LAST; i++) { 33 | #if 0 // DEBUG 34 | char buf[50]; 35 | sprintf(buf, "Write reg %02X = %02X\n", cmd[i].reg, cmd[i].value); 36 | OV7670_print(buf); 37 | #endif 38 | OV7670_write_register(platform, cmd[i].reg, cmd[i].value); 39 | OV7670_delay_ms(1); // Required, else lockup on init 40 | } 41 | } 42 | 43 | // CAMERA STARTUP ---------------------------------------------------------- 44 | 45 | static const OV7670_command 46 | OV7670_rgb[] = 47 | { 48 | // Manual output format, RGB, use RGB565 and full 0-255 output range 49 | {OV7670_REG_COM7, OV7670_COM7_RGB}, 50 | {OV7670_REG_RGB444, 0}, 51 | {OV7670_REG_COM15, OV7670_COM15_RGB565 | OV7670_COM15_R00FF}, 52 | {0xFF, 0xFF}}, 53 | OV7670_yuv[] = 54 | { 55 | // Manual output format, YUV, use full output range 56 | {OV7670_REG_COM7, OV7670_COM7_YUV}, 57 | {OV7670_REG_COM15, OV7670_COM15_R00FF}, 58 | {0xFF, 0xFF}}, 59 | OV7670_init[] = { 60 | {OV7670_REG_TSLB, OV7670_TSLB_YLAST}, // No auto window 61 | //{OV7670_REG_COM10, OV7670_COM10_VS_NEG}, // -VSYNC (req by SAMD PCC) 62 | {OV7670_REG_SLOP, 0x20}, 63 | {OV7670_REG_GAM_BASE, 0x1C}, 64 | {OV7670_REG_GAM_BASE + 1, 0x28}, 65 | {OV7670_REG_GAM_BASE + 2, 0x3C}, 66 | {OV7670_REG_GAM_BASE + 3, 0x55}, 67 | {OV7670_REG_GAM_BASE + 4, 0x68}, 68 | {OV7670_REG_GAM_BASE + 5, 0x76}, 69 | {OV7670_REG_GAM_BASE + 6, 0x80}, 70 | {OV7670_REG_GAM_BASE + 7, 0x88}, 71 | {OV7670_REG_GAM_BASE + 8, 0x8F}, 72 | {OV7670_REG_GAM_BASE + 9, 0x96}, 73 | {OV7670_REG_GAM_BASE + 10, 0xA3}, 74 | {OV7670_REG_GAM_BASE + 11, 0xAF}, 75 | {OV7670_REG_GAM_BASE + 12, 0xC4}, 76 | {OV7670_REG_GAM_BASE + 13, 0xD7}, 77 | {OV7670_REG_GAM_BASE + 14, 0xE8}, 78 | {OV7670_REG_COM8, 79 | OV7670_COM8_FASTAEC | OV7670_COM8_AECSTEP | OV7670_COM8_BANDING}, 80 | {OV7670_REG_GAIN, 0x00}, 81 | {OV7670_REG_COM2, 0x00}, 82 | {OV7670_REG_COM4, 0x00}, 83 | {OV7670_REG_COM9, 0x20}, // Max AGC value 84 | {OV7670_REG_COM11, (1 << 3)}, // 50Hz 85 | //{0x9D, 99}, // Banding filter for 50 Hz at 15.625 MHz 86 | {0x9D, 89}, // Banding filter for 50 Hz at 13.888 MHz 87 | {OV7670_REG_BD50MAX, 0x05}, 88 | {OV7670_REG_BD60MAX, 0x07}, 89 | {OV7670_REG_AEW, 0x75}, 90 | {OV7670_REG_AEB, 0x63}, 91 | {OV7670_REG_VPT, 0xA5}, 92 | {OV7670_REG_HAECC1, 0x78}, 93 | {OV7670_REG_HAECC2, 0x68}, 94 | {0xA1, 0x03}, // Reserved register? 95 | {OV7670_REG_HAECC3, 0xDF}, // Histogram-based AEC/AGC setup 96 | {OV7670_REG_HAECC4, 0xDF}, 97 | {OV7670_REG_HAECC5, 0xF0}, 98 | {OV7670_REG_HAECC6, 0x90}, 99 | {OV7670_REG_HAECC7, 0x94}, 100 | {OV7670_REG_COM8, OV7670_COM8_FASTAEC | OV7670_COM8_AECSTEP | 101 | OV7670_COM8_BANDING | OV7670_COM8_AGC | 102 | OV7670_COM8_AEC | OV7670_COM8_AWB }, 103 | {OV7670_REG_COM5, 0x61}, 104 | {OV7670_REG_COM6, 0x4B}, 105 | {0x16, 0x02}, // Reserved register? 106 | {OV7670_REG_MVFP, 0x07}, // 0x07, 107 | {OV7670_REG_ADCCTR1, 0x02}, 108 | {OV7670_REG_ADCCTR2, 0x91}, 109 | {0x29, 0x07}, // Reserved register? 110 | {OV7670_REG_CHLF, 0x0B}, 111 | {0x35, 0x0B}, // Reserved register? 112 | {OV7670_REG_ADC, 0x1D}, 113 | {OV7670_REG_ACOM, 0x71}, 114 | {OV7670_REG_OFON, 0x2A}, 115 | {OV7670_REG_COM12, 0x78}, 116 | {0x4D, 0x40}, // Reserved register? 117 | {0x4E, 0x20}, // Reserved register? 118 | {OV7670_REG_GFIX, 0x5D}, 119 | {OV7670_REG_REG74, 0x19}, 120 | {0x8D, 0x4F}, // Reserved register? 121 | {0x8E, 0x00}, // Reserved register? 122 | {0x8F, 0x00}, // Reserved register? 123 | {0x90, 0x00}, // Reserved register? 124 | {0x91, 0x00}, // Reserved register? 125 | {OV7670_REG_DM_LNL, 0x00}, 126 | {0x96, 0x00}, // Reserved register? 127 | {0x9A, 0x80}, // Reserved register? 128 | {0xB0, 0x84}, // Reserved register? 129 | {OV7670_REG_ABLC1, 0x0C}, 130 | {0xB2, 0x0E}, // Reserved register? 131 | {OV7670_REG_THL_ST, 0x82}, 132 | {0xB8, 0x0A}, // Reserved register? 133 | {OV7670_REG_AWBC1, 0x14}, 134 | {OV7670_REG_AWBC2, 0xF0}, 135 | {OV7670_REG_AWBC3, 0x34}, 136 | {OV7670_REG_AWBC4, 0x58}, 137 | {OV7670_REG_AWBC5, 0x28}, 138 | {OV7670_REG_AWBC6, 0x3A}, 139 | {0x59, 0x88}, // Reserved register? 140 | {0x5A, 0x88}, // Reserved register? 141 | {0x5B, 0x44}, // Reserved register? 142 | {0x5C, 0x67}, // Reserved register? 143 | {0x5D, 0x49}, // Reserved register? 144 | {0x5E, 0x0E}, // Reserved register? 145 | {OV7670_REG_LCC3, 0x04}, 146 | {OV7670_REG_LCC4, 0x20}, 147 | {OV7670_REG_LCC5, 0x05}, 148 | {OV7670_REG_LCC6, 0x04}, 149 | {OV7670_REG_LCC7, 0x08}, 150 | {OV7670_REG_AWBCTR3, 0x0A}, 151 | {OV7670_REG_AWBCTR2, 0x55}, 152 | //{OV7670_REG_MTX1, 0x80}, 153 | //{OV7670_REG_MTX2, 0x80}, 154 | //{OV7670_REG_MTX3, 0x00}, 155 | //{OV7670_REG_MTX4, 0x22}, 156 | //{OV7670_REG_MTX5, 0x5E}, 157 | //{OV7670_REG_MTX6, 0x80}, // 0x40? 158 | {OV7670_REG_AWBCTR1, 0x11}, 159 | //{OV7670_REG_AWBCTR0, 0x9F}, // Or use 0x9E for advance AWB 160 | {OV7670_REG_AWBCTR0, 0x9E}, // Or use 0x9E for advance AWB 161 | {OV7670_REG_BRIGHT, 0x00}, 162 | {OV7670_REG_CONTRAS, 0x40}, 163 | {OV7670_REG_CONTRAS_CENTER, 0x80}, // 0x40? 164 | {OV7670_REG_LAST + 1, 0x00}, // End-of-data marker 165 | }; 166 | 167 | OV7670_status OV7670_begin(OV7670_host *host, OV7670_colorspace colorspace, 168 | OV7670_size size, float fps) { 169 | OV7670_status status; 170 | 171 | // I2C must already be set up and running (@ 100 KHz) in calling code 172 | 173 | // Do device-specific (but platform-agnostic) setup. e.g. on SAMD this 174 | // function will fiddle registers to start a timer for XCLK output and 175 | // enable the parallel capture peripheral. 176 | /* 177 | status = OV7670_arch_begin(host); 178 | if (status != OV7670_STATUS_OK) { 179 | return status; 180 | } 181 | */ 182 | 183 | // Unsure of camera startup time from beginning of input clock. 184 | // Let's guess it's similar to tS:REG (300 ms) from datasheet. 185 | OV7670_delay_ms(300); 186 | 187 | // ENABLE AND/OR RESET CAMERA -------------------------------------------- 188 | 189 | if (host->pins->enable >= 0) { // Enable pin defined? 190 | OV7670_pin_output(host->pins->enable) 191 | OV7670_pin_write(host->pins->enable, 0); // PWDN low (enable) 192 | OV7670_delay_ms(300); 193 | } 194 | 195 | if (host->pins->reset >= 0) { // Hard reset pin defined? 196 | OV7670_pin_output(host->pins->reset); 197 | OV7670_pin_write(host->pins->reset, 0); 198 | OV7670_delay_ms(1); 199 | OV7670_pin_write(host->pins->reset, 1); 200 | } else { // Soft reset, doesn't seem reliable, might just need more delay? 201 | OV7670_write_register(host->platform, OV7670_REG_COM7, OV7670_COM7_RESET); 202 | } 203 | OV7670_delay_ms(1000); // Datasheet: tS:RESET = 1 ms 204 | 205 | //(void)OV7670_set_fps(host->platform, fps); // Timing 206 | OV7670_write_register(host->platform, OV7670_REG_CLKRC, 1); // CLK * 4 207 | OV7670_write_register(host->platform, OV7670_REG_DBLV, 1 << 6); // CLK / 4 208 | OV7670_set_format(host->platform, colorspace); 209 | OV7670_write_list(host->platform, OV7670_init); // Other config 210 | OV7670_set_size(host->platform, size); // Frame size 211 | 212 | OV7670_delay_ms(300); // tS:REG = 300 ms (settling time = 10 frames) 213 | 214 | return OV7670_STATUS_OK; 215 | } 216 | 217 | // MISCELLANY AND CAMERA CONFIG FUNCTIONS ---------------------------------- 218 | 219 | // Configure camera frame rate. Actual resulting frame rate (returned) may 220 | // be different depending on available clock frequencies. Result will only 221 | // exceed input if necessary for minimum supported rate, but this is very 222 | // rare, typically below 1 fps. In all other cases, result will be equal 223 | // or less than the requested rate, up to a maximum of 30 fps (the "or less" 224 | // is because requested fps may be based on other host hardware timing 225 | // constraints (e.g. screen) and rounding up to a closer-but-higher frame 226 | // rate would be problematic). There is no hardcoded set of fixed frame 227 | // rates because it varies with architecture, depending on OV7670_XCLK_HZ. 228 | // If platform is NULL, no registers are set, a fps request/return can be 229 | // evaluated without reconfiguring the camera, or without it even started. 230 | 231 | OV7670_status OV7670_set_format(void *platform, OV7670_colorspace colorspace) { 232 | if (colorspace == OV7670_COLOR_RGB) { 233 | OV7670_write_list(platform, OV7670_rgb); 234 | } else { 235 | OV7670_write_list(platform, OV7670_yuv); 236 | } 237 | 238 | return OV7670_STATUS_OK; 239 | } 240 | 241 | float OV7670_set_fps(void *platform, float fps) { 242 | 243 | // Pixel clock (PCLK), which determines overall frame rate, is a 244 | // function of XCLK input frequency (OV7670_XCLK_HZ), a PLL multiplier 245 | // and then an integer division factor (1-32). These are the available 246 | // OV7670 PLL ratios: 247 | static const uint8_t pll_ratio[] = {1, 4, 6, 8}; 248 | const uint8_t num_plls = sizeof pll_ratio / sizeof pll_ratio[0]; 249 | 250 | // Constrain frame rate to upper and lower limits 251 | fps = (fps > 30) ? 30 : fps; // Max 30 FPS 252 | float pclk_target = fps * 4000000.0 / 5.0; // Ideal PCLK Hz for target FPS 253 | uint32_t pclk_min = OV7670_XCLK_HZ / 32; // Min PCLK determines min FPS 254 | if (pclk_target < (float)pclk_min) { // If PCLK target is below limit 255 | if (platform) { 256 | OV7670_write_register(platform, OV7670_REG_DBLV, 0); // 1:1 PLL 257 | OV7670_write_register(platform, OV7670_REG_CLKRC, 31); // 1/32 div 258 | } 259 | return (float)(pclk_min * 5 / 4000000); // Return min frame rate 260 | } 261 | 262 | // Find nearest available FPS without going over. This is done in a 263 | // brute-force manner, testing all 127 PLL-up by divide-down permutations 264 | // and tracking the best fit. I’m certain there are shortcuts but was 265 | // having trouble with my math, might revisit later. It's not a huge 266 | // bottleneck...MCUs are fast now, many cases are quickly discarded, and 267 | // this operation is usually done only once on startup (the I2C transfers 268 | // probably take longer). 269 | 270 | uint8_t best_pll = 0; // Index (not value) of best PLL match 271 | uint8_t best_div = 1; // Value of best division factor match 272 | float best_delta = 30.0; // Best requested vs actual FPS (init to "way off") 273 | 274 | for (uint8_t p = 0; p < num_plls; p++) { 275 | uint32_t xclk_pll = OV7670_XCLK_HZ * pll_ratio[p]; // PLL'd freq 276 | uint8_t first_div = p ? 2 : 1; // Min div is 1 for PLL 1:1, else 2 277 | for (uint8_t div = first_div; div <= 32; div++) { 278 | uint32_t pclk_result = xclk_pll / div; // PCLK-up-down permutation 279 | if (pclk_result > pclk_target) { // Exceeds target? 280 | continue; // Skip it 281 | } 282 | float fps_result = (float)pclk_result * 5.0 / 4000000.0; 283 | float delta = fps - fps_result; // How far off? 284 | if (delta < best_delta) { // Best match yet? 285 | best_delta = delta; // Save delta, 286 | best_pll = p; // pll and 287 | best_div = div; // div for later use 288 | } 289 | } 290 | } 291 | 292 | if (platform) { 293 | // Set up DBLV and CLKRC registers with best PLL and div values 294 | if (pll_ratio[best_pll] == best_div) { // If PLL and div are same (1:1) 295 | // Bypass PLL, use external clock directly 296 | OV7670_write_register(platform, OV7670_REG_DBLV, 0); 297 | OV7670_write_register(platform, OV7670_REG_CLKRC, 0x40); 298 | } else { 299 | // Set DBLV[7:6] for PLL, CLKRC[5:0] for div-1 (1-32 stored as 0-31) 300 | OV7670_write_register(platform, OV7670_REG_DBLV, best_pll << 6); 301 | OV7670_write_register(platform, OV7670_REG_CLKRC, best_div - 1); 302 | } 303 | } 304 | 305 | return fps - best_delta; // Return actual frame rate 306 | } 307 | 308 | // Sets up PCLK dividers and sets H/V start/stop window. Rather than 309 | // rolling this into OV7670_set_size(), it's kept separate so test code 310 | // can experiment with different settings to find ideal defaults. 311 | void OV7670_frame_control(void *platform, uint8_t size, uint8_t vstart, 312 | uint16_t hstart, uint8_t edge_offset, 313 | uint8_t pclk_delay) { 314 | uint8_t value; 315 | 316 | // Enable downsampling if sub-VGA, and zoom if 1:16 scale 317 | value = (size > OV7670_SIZE_DIV1) ? OV7670_COM3_DCWEN : 0; 318 | if (size == OV7670_SIZE_DIV16) 319 | value |= OV7670_COM3_SCALEEN; 320 | OV7670_write_register(platform, OV7670_REG_COM3, value); 321 | 322 | // Enable PCLK division if sub-VGA 2,4,8,16 = 0x19,1A,1B,1C 323 | value = (size > OV7670_SIZE_DIV1) ? (0x18 + size) : 0; 324 | OV7670_write_register(platform, OV7670_REG_COM14, value); 325 | 326 | // Horiz/vert downsample ratio, 1:8 max (H,V are always equal for now) 327 | value = (size <= OV7670_SIZE_DIV8) ? size : OV7670_SIZE_DIV8; 328 | OV7670_write_register(platform, OV7670_REG_SCALING_DCWCTR, value * 0x11); 329 | 330 | // Pixel clock divider if sub-VGA 331 | value = (size > OV7670_SIZE_DIV1) ? (0xF0 + size) : 0x08; 332 | OV7670_write_register(platform, OV7670_REG_SCALING_PCLK_DIV, value); 333 | 334 | // Apply 0.5 digital zoom at 1:16 size (others are downsample only) 335 | value = (size == OV7670_SIZE_DIV16) ? 0x40 : 0x20; // 0.5, 1.0 336 | // Read current SCALING_XSC and SCALING_YSC register values because 337 | // test pattern settings are also stored in those registers and we 338 | // don't want to corrupt anything there. 339 | uint8_t xsc = OV7670_read_register(platform, OV7670_REG_SCALING_XSC); 340 | uint8_t ysc = OV7670_read_register(platform, OV7670_REG_SCALING_YSC); 341 | xsc = (xsc & 0x80) | value; // Modify only scaling bits (not test pattern) 342 | ysc = (ysc & 0x80) | value; 343 | // Write modified result back to SCALING_XSC and SCALING_YSC 344 | OV7670_write_register(platform, OV7670_REG_SCALING_XSC, xsc); 345 | OV7670_write_register(platform, OV7670_REG_SCALING_YSC, ysc); 346 | 347 | // Window size is scattered across multiple registers. 348 | // Horiz/vert stops can be automatically calc'd from starts. 349 | uint16_t vstop = vstart + 480; 350 | uint16_t hstop = (hstart + 640) % 784; 351 | OV7670_write_register(platform, OV7670_REG_HSTART, hstart >> 3); 352 | OV7670_write_register(platform, OV7670_REG_HSTOP, hstop >> 3); 353 | OV7670_write_register(platform, OV7670_REG_HREF, 354 | (edge_offset << 6) | ((hstop & 0b111) << 3) | 355 | (hstart & 0b111)); 356 | OV7670_write_register(platform, OV7670_REG_VSTART, vstart >> 2); 357 | OV7670_write_register(platform, OV7670_REG_VSTOP, vstop >> 2); 358 | OV7670_write_register(platform, OV7670_REG_VREF, 359 | ((vstop & 0b11) << 2) | (vstart & 0b11)); 360 | 361 | OV7670_write_register(platform, OV7670_REG_SCALING_PCLK_DELAY, pclk_delay); 362 | } 363 | 364 | void OV7670_set_size(void *platform, OV7670_size size) { 365 | // Array of five window settings, index of each (0-4) aligns with the five 366 | // OV7670_size enumeration values. If enum changes, list must change! 367 | static struct { 368 | uint8_t vstart; 369 | uint8_t hstart; 370 | uint8_t edge_offset; 371 | uint8_t pclk_delay; 372 | } window[] = { 373 | // Window settings were tediously determined empirically. 374 | // I hope there's a formula for this, if a do-over is needed. 375 | {9, 162, 2, 2}, // SIZE_DIV1 640x480 VGA 376 | {10, 174, 4, 2}, // SIZE_DIV2 320x240 QVGA 377 | {11, 186, 2, 2}, // SIZE_DIV4 160x120 QQVGA 378 | {12, 210, 0, 2}, // SIZE_DIV8 80x60 ... 379 | {15, 252, 3, 2}, // SIZE_DIV16 40x30 380 | }; 381 | 382 | OV7670_frame_control(platform, size, window[size].vstart, window[size].hstart, 383 | window[size].edge_offset, window[size].pclk_delay); 384 | } 385 | 386 | // Select one of the camera's night modes (or disable). 387 | // Trades off frame rate for less grainy images in low light. 388 | // Note: seems that frame rate is somewhat automatic despite 389 | // the requested setting, i.e. if 1:8 is selected, might 390 | // still get normal frame rate or something higher than 391 | // 1:8, if the scene lighting permits. Also the setting 392 | // seems to 'stick' in some cases when trying to turn 393 | // this off. Might want to try always having night mode 394 | // enabled but using 1:1 frame setting as 'off'. 395 | void OV7670_night(void *platform, OV7670_night_mode night) { 396 | // Table of bit patterns for the different supported night modes. 397 | // There's a "same frame rate" option for OV7670 night mode but it 398 | // doesn't seem to do anything useful and can be skipped over. 399 | static const uint8_t night_bits[] = {0b00000000, 0b10100000, 0b11000000, 400 | 0b11100000}; 401 | // Read current COM11 register setting so unrelated bits aren't corrupted 402 | uint8_t com11 = OV7670_read_register(platform, OV7670_REG_COM11); 403 | com11 &= 0b00011111; // Clear night mode bits 404 | com11 |= night_bits[night]; // Set night bits for desired mode 405 | // Write modified result back to COM11 register 406 | OV7670_write_register(platform, OV7670_REG_COM11, com11); 407 | } 408 | 409 | // Flips camera output on horizontal and/or vertical axes. 410 | // Note: datasheet refers to horizontal flip as "mirroring," but 411 | // avoiding that terminology here that it might be mistaken for a 412 | // split-down-middle-and-reflect funhouse effect, which it isn't. 413 | // Also note: mirrored image isn't always centered quite the same, 414 | // looks like frame control settings might need to be tweaked 415 | // depending on flips. Similar issue to color bars? 416 | void OV7670_flip(void *platform, bool flip_x, bool flip_y) { 417 | // Read current MVFP register setting, so we don't corrupt any 418 | // reserved bits or the "black sun" bit if it was previously set. 419 | uint8_t mvfp = OV7670_read_register(platform, OV7670_REG_MVFP); 420 | if (flip_x) { 421 | mvfp |= OV7670_MVFP_MIRROR; // Horizontal flip 422 | } else { 423 | mvfp &= ~OV7670_MVFP_MIRROR; 424 | } 425 | if (flip_y) { 426 | mvfp |= OV7670_MVFP_VFLIP; // Vertical flip 427 | } else { 428 | mvfp &= ~OV7670_MVFP_VFLIP; 429 | } 430 | // Write modified result back to MVFP register 431 | OV7670_write_register(platform, OV7670_REG_MVFP, mvfp); 432 | } 433 | 434 | // Selects one of the camera's test patterns (or disable). 435 | // See Adafruit_OV7670.h for notes about minor visual bug here. 436 | void OV7670_test_pattern(void *platform, OV7670_pattern pattern) { 437 | // Read current SCALING_XSC and SCALING_YSC register settings, 438 | // so image scaling settings aren't corrupted. 439 | uint8_t xsc = OV7670_read_register(platform, OV7670_REG_SCALING_XSC); 440 | uint8_t ysc = OV7670_read_register(platform, OV7670_REG_SCALING_YSC); 441 | if (pattern & 1) { 442 | xsc |= 0x80; 443 | } else { 444 | xsc &= ~0x80; 445 | } 446 | if (pattern & 2) { 447 | ysc |= 0x80; 448 | } else { 449 | ysc &= ~0x80; 450 | } 451 | // Write modified results back to SCALING_XSC and SCALING_YSC registers 452 | OV7670_write_register(platform, OV7670_REG_SCALING_XSC, xsc); 453 | OV7670_write_register(platform, OV7670_REG_SCALING_YSC, ysc); 454 | } 455 | 456 | // Reformat YUV gray component to RGB565 for TFT preview. 457 | // Big-endian in and out. 458 | void OV7670_Y2RGB565(uint16_t *ptr, uint32_t len) { 459 | while (len--) { 460 | uint8_t y = *ptr & 0xFF; // Y (brightness) component of YUV 461 | uint16_t rgb = ((y >> 3) * 0x801) | ((y & 0xFC) << 3); // to RGB565 462 | *ptr++ = __builtin_bswap16(rgb); // Big-endianify RGB565 for TFT 463 | } 464 | } 465 | --------------------------------------------------------------------------------