diff --git a/src/micropython b/src/micropython index 25402f4ce..7e9eeadd0 160000 --- a/src/micropython +++ b/src/micropython @@ -1 +1 @@ -Subproject commit 25402f4ce908d6f76180a0bde3c9f1f03b61a459 +Subproject commit 7e9eeadd0a64f9f69f3447d0c090349595e3071f diff --git a/src/omv/boards/NANO33/omv_boardconfig.h b/src/omv/boards/NANO33/omv_boardconfig.h index 3968b2ef1..5edb318cc 100644 --- a/src/omv/boards/NANO33/omv_boardconfig.h +++ b/src/omv/boards/NANO33/omv_boardconfig.h @@ -44,6 +44,7 @@ // Enable sensor drivers #define OMV_ENABLE_OV2640 (0) #define OMV_ENABLE_OV5640 (0) +#define OMV_ENABLE_OV7670 (1) #define OMV_ENABLE_OV7690 (0) #define OMV_ENABLE_OV7725 (0) #define OMV_ENABLE_OV9650 (0) @@ -62,11 +63,7 @@ // If buffer size is bigger than this threshold, the quality is reduced. // This is only used for JPEG images sent to the IDE not normal compression. -#define JPEG_QUALITY_THRESH (320*240*2) - -// Low and high JPEG QS. -#define JPEG_QUALITY_LOW 50 -#define JPEG_QUALITY_HIGH 90 +#define JPEG_QUALITY_THRESH (320*240) // Low and high JPEG QS. #define JPEG_QUALITY_LOW 50 @@ -103,6 +100,12 @@ #define FIR_I2C_SDA_PIN (31) #define FIR_I2C_SPEED (CAMBUS_SPEED_FULL) +// ISC I2C +#define ISC_I2C_ID (0) +#define ISC_I2C_SCL_PIN (2) +#define ISC_I2C_SDA_PIN (31) +#define ISC_I2C_SPEED (CAMBUS_SPEED_FULL) + // I2C0 #define TWI0_ID (0) #define TWI0_SCL_PIN (2) @@ -120,6 +123,40 @@ #define PDM_CLK_PIN (26) #define PDM_PWR_PIN (17) +// DCMI +#define DCMI_PWDN_PIN (29) +//#define DCMI_RESET_PIN (30) + +#define DCMI_D0_PIN (32+2) +#define DCMI_D1_PIN (32+3) +#define DCMI_D2_PIN (32+10) +#define DCMI_D3_PIN (32+11) +#define DCMI_D4_PIN (32+12) +#define DCMI_D5_PIN (32+13) +#define DCMI_D6_PIN (32+14) +#define DCMI_D7_PIN (32+15) + +#define DCMI_VSYNC_PIN (21) +#define DCMI_HSYNC_PIN (5) +#define DCMI_PXCLK_PIN (4) +#define DCMI_XCLK_PIN (27) + +#if defined(DCMI_RESET_PIN) +#define DCMI_RESET_LOW() nrf_gpio_pin_clear(DCMI_RESET_PIN) +#define DCMI_RESET_HIGH() nrf_gpio_pin_set(DCMI_RESET_PIN) +#else +#define DCMI_RESET_LOW() +#define DCMI_RESET_HIGH() +#endif + +#if defined(DCMI_PWDN_PIN) +#define DCMI_PWDN_LOW() nrf_gpio_pin_clear(DCMI_PWDN_PIN) +#define DCMI_PWDN_HIGH() nrf_gpio_pin_set(DCMI_PWDN_PIN) +#else +#define DCMI_PWDN_LOW() +#define DCMI_PWDN_HIGH() +#endif + // FIR Module #define OMV_ENABLE_FIR_MLX90621 (1) #define OMV_ENABLE_FIR_MLX90640 (1) diff --git a/src/omv/boards/NANO33/omv_boardconfig.mk b/src/omv/boards/NANO33/omv_boardconfig.mk index 3c4be3ae1..0d6eb9b93 100755 --- a/src/omv/boards/NANO33/omv_boardconfig.mk +++ b/src/omv/boards/NANO33/omv_boardconfig.mk @@ -16,3 +16,4 @@ MICROPY_PY_ULAB=1 MICROPY_PY_AUDIO=1 MICROPY_PY_LCD = 0 MICROPY_PY_TV = 0 +MICROPY_PY_SENSOR = 1 diff --git a/src/omv/ports/stm32/sensor.h b/src/omv/common/sensor.h similarity index 100% rename from src/omv/ports/stm32/sensor.h rename to src/omv/common/sensor.h diff --git a/src/omv/ports/nrf/main.c b/src/omv/ports/nrf/main.c index 7edb1f216..77fab4672 100644 --- a/src/omv/ports/nrf/main.c +++ b/src/omv/ports/nrf/main.c @@ -81,6 +81,8 @@ #include "py_audio.h" #include "framebuffer.h" #include "omv_boardconfig.h" +#include "cambus.h" +#include "sensor.h" uint32_t HAL_GetHalVersion() { @@ -151,7 +153,11 @@ soft_reset: uart_init0(); #endif + #if MICROPY_PY_SENSOR + sensor_init(); + #endif framebuffer_init0(); + fb_alloc_init0(); #if (MICROPY_PY_BLE_NUS == 0) && (MICROPY_HW_USB_CDC == 0) { diff --git a/src/omv/ports/nrf/sensor.c b/src/omv/ports/nrf/sensor.c new file mode 100644 index 000000000..367a7f6b8 --- /dev/null +++ b/src/omv/ports/nrf/sensor.c @@ -0,0 +1,1060 @@ +/* + * This file is part of the OpenMV project. + * + * Copyright (c) 2013-2021 Ibrahim Abdelkader + * Copyright (c) 2013-2021 Kwabena W. Agyeman + * + * This work is licensed under the MIT license, see the file LICENSE for details. + * + * Sensor abstraction layer for nRF port. + */ +#include +#include +#include +#include "py/mphal.h" +#include "cambus.h" +#include "sensor.h" +#include "ov2640.h" +#include "ov5640.h" +#include "ov7725.h" +#include "ov7690.h" +#include "ov7670.h" +#include "ov9650.h" +#include "mt9v034.h" +#include "lepton.h" +#include "hm01b0.h" +#include "framebuffer.h" +#include "omv_boardconfig.h" +#include "unaligned_memcpy.h" +#include "nrf_i2s.h" +#include "hal/nrf_gpio.h" + +extern void __fatal_error(const char *msg); + +sensor_t sensor = {0}; + +static uint32_t _vsyncMask; +static uint32_t _hrefMask; +static uint32_t _pclkMask; +static const volatile uint32_t *_vsyncPort; +static const volatile uint32_t *_hrefPort; +static const volatile uint32_t *_pclkPort; + +#define interrupts() __enable_irq() +#define noInterrupts() __disable_irq() + +#define digitalPinToPort(P) (P/32) + +#ifndef digitalPinToBitMask +#define digitalPinToBitMask(P) (1 << (P % 32)) +#endif + +#ifndef portInputRegister +#define portInputRegister(P) ((P == 0) ? &NRF_P0->IN : &NRF_P1->IN) +#endif + +const int resolution[][2] = { + {0, 0 }, + // C/SIF Resolutions + {88, 72 }, /* QQCIF */ + {176, 144 }, /* QCIF */ + {352, 288 }, /* CIF */ + {88, 60 }, /* QQSIF */ + {176, 120 }, /* QSIF */ + {352, 240 }, /* SIF */ + // VGA Resolutions + {40, 30 }, /* QQQQVGA */ + {80, 60 }, /* QQQVGA */ + {160, 120 }, /* QQVGA */ + {320, 240 }, /* QVGA */ + {640, 480 }, /* VGA */ + {60, 40 }, /* HQQQVGA */ + {120, 80 }, /* HQQVGA */ + {240, 160 }, /* HQVGA */ + // FFT Resolutions + {64, 32 }, /* 64x32 */ + {64, 64 }, /* 64x64 */ + {128, 64 }, /* 128x64 */ + {128, 128 }, /* 128x128 */ + // Other + {128, 160 }, /* LCD */ + {128, 160 }, /* QQVGA2 */ + {720, 480 }, /* WVGA */ + {752, 480 }, /* WVGA2 */ + {800, 600 }, /* SVGA */ + {1024, 768 }, /* XGA */ + {1280, 1024}, /* SXGA */ + {1600, 1200}, /* UXGA */ + {1280, 720 }, /* HD */ + {1920, 1080}, /* FHD */ + {2560, 1440}, /* QHD */ + {2048, 1536}, /* QXGA */ + {2560, 1600}, /* WQXGA */ + {2592, 1944}, /* WQXGA2 */ +}; + +static int extclk_config(int frequency) +{ + #ifndef I2S_CONFIG_MCKFREQ_MCKFREQ_32MDIV2 + // Note this define is out of spec and has been removed from hal. + #define I2S_CONFIG_MCKFREQ_MCKFREQ_32MDIV2 (0x80000000UL) /*!< 32 MHz / 2 = 16.0 MHz */ + #endif + + nrf_gpio_cfg_output(DCMI_XCLK_PIN); + + // Generates 16 MHz signal using I2S peripheral + NRF_I2S->CONFIG.MCKEN = (I2S_CONFIG_MCKEN_MCKEN_ENABLE << I2S_CONFIG_MCKEN_MCKEN_Pos); + NRF_I2S->CONFIG.MCKFREQ = I2S_CONFIG_MCKFREQ_MCKFREQ_32MDIV2 << I2S_CONFIG_MCKFREQ_MCKFREQ_Pos; + NRF_I2S->CONFIG.MODE = I2S_CONFIG_MODE_MODE_MASTER << I2S_CONFIG_MODE_MODE_Pos; + + NRF_I2S->PSEL.MCK = (DCMI_XCLK_PIN << I2S_PSEL_MCK_PIN_Pos); + + NRF_I2S->ENABLE = 1; + NRF_I2S->TASKS_START = 1; + + return 0; +} + +static int dcmi_config() +{ + uint32_t dcmi_pins[] = { + DCMI_D0_PIN, + DCMI_D1_PIN, + DCMI_D2_PIN, + DCMI_D3_PIN, + DCMI_D4_PIN, + DCMI_D5_PIN, + DCMI_D6_PIN, + DCMI_D7_PIN, + DCMI_VSYNC_PIN, + DCMI_HSYNC_PIN, + DCMI_PXCLK_PIN, + }; + + // Configure DCMI input pins + for (int i=0; ienabled = 0; + + // Set default color palette. + sensor.color_palette = rainbow_table; + + sensor.detected = true; + + // Disable VSYNC IRQ and callback + sensor_set_vsync_callback(NULL); + + /* All good! */ + return 0; +} + +int sensor_reset() +{ + // Reset the sensor state + sensor.sde = 0; + sensor.pixformat = 0; + sensor.framesize = 0; + sensor.framerate = 0; + sensor.gainceiling = 0; + sensor.hmirror = false; + sensor.vflip = false; + sensor.transpose = false; + #if MICROPY_PY_IMU + sensor.auto_rotation = sensor.chip_id == OV7690_ID; + #else + sensor.auto_rotation = false; + #endif // MICROPY_PY_IMU + sensor.vsync_callback= NULL; + + // Reset default color palette. + sensor.color_palette = rainbow_table; + + // Restore shutdown state on reset. + sensor_shutdown(false); + + // Hard-reset the sensor + if (sensor.reset_pol == ACTIVE_HIGH) { + DCMI_RESET_HIGH(); + mp_hal_delay_ms(10); + DCMI_RESET_LOW(); + } else { + DCMI_RESET_LOW(); + mp_hal_delay_ms(10); + DCMI_RESET_HIGH(); + } + mp_hal_delay_ms(20); + + // Call sensor-specific reset function + if (sensor.reset(&sensor) != 0) { + return -1; + } + + return 0; +} + +int sensor_get_id() +{ + return sensor.chip_id; +} + +bool sensor_is_detected() +{ + return sensor.detected; +} + +int sensor_sleep(int enable) +{ + if (sensor.sleep == NULL + || sensor.sleep(&sensor, enable) != 0) { + // Operation not supported + return -1; + } + return 0; +} + +int sensor_shutdown(int enable) +{ + int ret = 0; + if (enable) { + if (sensor.pwdn_pol == ACTIVE_HIGH) { + DCMI_PWDN_HIGH(); + } else { + DCMI_PWDN_LOW(); + } + } else { + if (sensor.pwdn_pol == ACTIVE_HIGH) { + DCMI_PWDN_LOW(); + } else { + DCMI_PWDN_HIGH(); + } + } + + mp_hal_delay_ms(10); + return ret; +} + +int sensor_read_reg(uint16_t reg_addr) +{ + if (sensor.read_reg == NULL) { + // Operation not supported + return -1; + } + return sensor.read_reg(&sensor, reg_addr); +} + +int sensor_write_reg(uint16_t reg_addr, uint16_t reg_data) +{ + if (sensor.write_reg == NULL) { + // Operation not supported + return -1; + } + return sensor.write_reg(&sensor, reg_addr, reg_data); +} + +int sensor_set_pixformat(pixformat_t pixformat) +{ + if (sensor.pixformat == pixformat) { + // No change + return 0; + } + + // sensor_check_buffsize() will switch from PIXFORMAT_BAYER to PIXFORMAT_RGB565 to try to fit + // the MAIN_FB() in RAM as a first step optimization. If the user tries to switch back to RGB565 + // and that would be bigger than the RAM buffer we would just switch back. + // + // So, just short-circuit doing any work. + // + // This code is explicitly here to allow users to set the resolution to RGB565 and have it + // switch to BAYER only once even though they are setting the resolution to RGB565 repeatedly + // in a loop. Only RGB565->BAYER has this problem and needs this fix because of sensor_check_buffsize(). + uint32_t size = framebuffer_get_buffer_size(); + if ((sensor.pixformat == PIXFORMAT_BAYER) + && (pixformat == PIXFORMAT_RGB565) + && (MAIN_FB()->u * MAIN_FB()->v * 2 > size) + && (MAIN_FB()->u * MAIN_FB()->v * 1 <= size)) { + // No change + return 0; + } + + // Cropping and transposing (and thus auto rotation) don't work in JPEG mode. + //if ((pixformat == PIXFORMAT_JPEG) && (cropped() || sensor.transpose || sensor.auto_rotation)) { + // return -1; + //} + + if (sensor.set_pixformat == NULL + || sensor.set_pixformat(&sensor, pixformat) != 0) { + // Operation not supported + return -1; + } + + mp_hal_delay_ms(100); // wait for the camera to settle + + // Set pixel format + sensor.pixformat = pixformat; + + // Skip the first frame. + MAIN_FB()->bpp = -1; + + return 0; +} + +int sensor_set_framesize(framesize_t framesize) +{ + if (sensor.framesize == framesize) { + // No change + return 0; + } + + // Call the sensor specific function + if (sensor.set_framesize == NULL + || sensor.set_framesize(&sensor, framesize) != 0) { + // Operation not supported + return -1; + } + + mp_hal_delay_ms(100); // wait for the camera to settle + + // Set framebuffer size + sensor.framesize = framesize; + + // Skip the first frame. + MAIN_FB()->bpp = -1; + + // Set MAIN FB x offset, y offset, width, height, backup width, and backup height. + MAIN_FB()->x = 0; + MAIN_FB()->y = 0; + MAIN_FB()->w = MAIN_FB()->u = resolution[framesize][0]; + MAIN_FB()->h = MAIN_FB()->v = resolution[framesize][1]; + + return 0; +} + +int sensor_set_framerate(int framerate) +{ + if (sensor.framerate == framerate) { + // No change + return 0; + } + + // Call the sensor specific function + if (sensor.set_framerate == NULL + || sensor.set_framerate(&sensor, framerate) != 0) { + // Operation not supported + return -1; + } + + return 0; +} + +int sensor_set_windowing(int x, int y, int w, int h) +{ + // py_sensor_set_windowing ensures this the window is at least 8x8 + // and that it is fully inside the sensor output framesize window. + if (sensor.pixformat == PIXFORMAT_JPEG) { + return -1; + } + + // We force everything to be a multiple of 2 so that when you switch between + // grayscale/rgb565/bayer/jpeg the frame doesn't need to move around for bayer to work. + MAIN_FB()->x = (x / 2) * 2; + MAIN_FB()->y = (y / 2) * 2; + MAIN_FB()->w = MAIN_FB()->u = (w / 2) * 2; + MAIN_FB()->h = MAIN_FB()->v = (h / 2) * 2; + + return 0; +} + +int sensor_set_contrast(int level) +{ + if (sensor.set_contrast != NULL) { + return sensor.set_contrast(&sensor, level); + } + return -1; +} + +int sensor_set_brightness(int level) +{ + if (sensor.set_brightness != NULL) { + return sensor.set_brightness(&sensor, level); + } + return -1; +} + +int sensor_set_saturation(int level) +{ + if (sensor.set_saturation != NULL) { + return sensor.set_saturation(&sensor, level); + } + return -1; +} + +int sensor_set_gainceiling(gainceiling_t gainceiling) +{ + if (sensor.gainceiling == gainceiling) { + /* no change */ + return 0; + } + + /* call the sensor specific function */ + if (sensor.set_gainceiling == NULL + || sensor.set_gainceiling(&sensor, gainceiling) != 0) { + /* operation not supported */ + return -1; + } + + sensor.gainceiling = gainceiling; + return 0; +} + +int sensor_set_quality(int qs) +{ + /* call the sensor specific function */ + if (sensor.set_quality == NULL + || sensor.set_quality(&sensor, qs) != 0) { + /* operation not supported */ + return -1; + } + return 0; +} + +int sensor_set_colorbar(int enable) +{ + /* call the sensor specific function */ + if (sensor.set_colorbar == NULL + || sensor.set_colorbar(&sensor, enable) != 0) { + /* operation not supported */ + return -1; + } + return 0; +} + +int sensor_set_auto_gain(int enable, float gain_db, float gain_db_ceiling) +{ + /* call the sensor specific function */ + if (sensor.set_auto_gain == NULL + || sensor.set_auto_gain(&sensor, enable, gain_db, gain_db_ceiling) != 0) { + /* operation not supported */ + return -1; + } + return 0; +} + +int sensor_get_gain_db(float *gain_db) +{ + /* call the sensor specific function */ + if (sensor.get_gain_db == NULL + || sensor.get_gain_db(&sensor, gain_db) != 0) { + /* operation not supported */ + return -1; + } + return 0; +} + +int sensor_set_auto_exposure(int enable, int exposure_us) +{ + /* call the sensor specific function */ + if (sensor.set_auto_exposure == NULL + || sensor.set_auto_exposure(&sensor, enable, exposure_us) != 0) { + /* operation not supported */ + return -1; + } + return 0; +} + +int sensor_get_exposure_us(int *exposure_us) +{ + /* call the sensor specific function */ + if (sensor.get_exposure_us == NULL + || sensor.get_exposure_us(&sensor, exposure_us) != 0) { + /* operation not supported */ + return -1; + } + return 0; +} + +int sensor_set_auto_whitebal(int enable, float r_gain_db, float g_gain_db, float b_gain_db) +{ + /* call the sensor specific function */ + if (sensor.set_auto_whitebal == NULL + || sensor.set_auto_whitebal(&sensor, enable, r_gain_db, g_gain_db, b_gain_db) != 0) { + /* operation not supported */ + return -1; + } + return 0; +} + +int sensor_get_rgb_gain_db(float *r_gain_db, float *g_gain_db, float *b_gain_db) +{ + /* call the sensor specific function */ + if (sensor.get_rgb_gain_db == NULL + || sensor.get_rgb_gain_db(&sensor, r_gain_db, g_gain_db, b_gain_db) != 0) { + /* operation not supported */ + return -1; + } + return 0; +} + +int sensor_set_hmirror(int enable) +{ + if (sensor.hmirror == ((bool) enable)) { + /* no change */ + return 0; + } + + /* call the sensor specific function */ + if (sensor.set_hmirror == NULL + || sensor.set_hmirror(&sensor, enable) != 0) { + /* operation not supported */ + return -1; + } + sensor.hmirror = enable; + mp_hal_delay_ms(100); // wait for the camera to settle + return 0; +} + +bool sensor_get_hmirror() +{ + return sensor.hmirror; +} + +int sensor_set_vflip(int enable) +{ + if (sensor.vflip == ((bool) enable)) { + /* no change */ + return 0; + } + + /* call the sensor specific function */ + if (sensor.set_vflip == NULL + || sensor.set_vflip(&sensor, enable) != 0) { + /* operation not supported */ + return -1; + } + sensor.vflip = enable; + mp_hal_delay_ms(100); // wait for the camera to settle + return 0; +} + +bool sensor_get_vflip() +{ + return sensor.vflip; +} + +int sensor_set_transpose(bool enable) +{ + if (sensor.pixformat == PIXFORMAT_JPEG) { + return -1; + } + + sensor.transpose = enable; + return 0; +} + +bool sensor_get_transpose() +{ + return sensor.transpose; +} + +int sensor_set_auto_rotation(bool enable) +{ + if (sensor.pixformat == PIXFORMAT_JPEG) { + return -1; + } + + sensor.auto_rotation = enable; + return 0; +} + +bool sensor_get_auto_rotation() +{ + return sensor.auto_rotation; +} + +int sensor_set_special_effect(sde_t sde) +{ + if (sensor.sde == sde) { + /* no change */ + return 0; + } + + /* call the sensor specific function */ + if (sensor.set_special_effect == NULL + || sensor.set_special_effect(&sensor, sde) != 0) { + /* operation not supported */ + return -1; + } + + sensor.sde = sde; + return 0; +} + +int sensor_set_lens_correction(int enable, int radi, int coef) +{ + /* call the sensor specific function */ + if (sensor.set_lens_correction == NULL + || sensor.set_lens_correction(&sensor, enable, radi, coef) != 0) { + /* operation not supported */ + return -1; + } + + return 0; +} + +int sensor_ioctl(int request, ... /* arg */) +{ + int ret = -1; + + if (sensor.ioctl != NULL) { + va_list ap; + va_start(ap, request); + /* call the sensor specific function */ + ret = sensor.ioctl(&sensor, request, ap); + va_end(ap); + } + + return ret; +} + +int sensor_set_vsync_callback(vsync_cb_t vsync_cb) +{ + sensor.vsync_callback = vsync_cb; + if (sensor.vsync_callback == NULL) { + // Disable VSYNC EXTI IRQ + } else { + // Enable VSYNC EXTI IRQ + } + return 0; +} + +int sensor_set_color_palette(const uint16_t *color_palette) +{ + sensor.color_palette = color_palette; + return 0; +} + +const uint16_t *sensor_get_color_palette() +{ + return sensor.color_palette; +} + +void VsyncExtiCallback() +{ + if (sensor.vsync_callback != NULL) { + //sensor.vsync_callback(HAL_GPIO_ReadPin(DCMI_VSYNC_PORT, DCMI_VSYNC_PIN)); + } +} + +// To make the user experience better we automatically shrink the size of the MAIN_FB() to fit +// within the RAM we have onboard the system. +void sensor_check_buffsize() +{ + uint32_t size = framebuffer_get_buffer_size(); + uint32_t bpp; + + switch (sensor.pixformat) { + case PIXFORMAT_GRAYSCALE: + case PIXFORMAT_BAYER: + bpp = 1; + break; + case PIXFORMAT_RGB565: + case PIXFORMAT_YUV422: + bpp = 2; + break; + // If the pixformat is NULL/JPEG there we can't do anything to check if it fits before hand. + default: + return; + } + + // MAIN_FB() fits, we are done. + if ((MAIN_FB()->u * MAIN_FB()->v * bpp) <= size) { + return; + } + + if (sensor.pixformat == PIXFORMAT_RGB565) { + // Switch to bayer for the quick 2x savings. + sensor_set_pixformat(PIXFORMAT_BAYER); + bpp = 1; + + // MAIN_FB() fits, we are done (bpp is 1). + if (MAIN_FB()->u * MAIN_FB()->v <= size) { + return; + } + } + + int window_w = MAIN_FB()->u; + int window_h = MAIN_FB()->v; + + // We need to shrink the frame buffer. We can do this by cropping. So, we will subtract columns + // and rows from the frame buffer until it fits within the frame buffer. + int max = IM_MAX(window_w, window_h); + int min = IM_MIN(window_w, window_h); + float aspect_ratio = max / ((float) min); + float r = aspect_ratio, best_r = r; + int c = 1, best_c = c; + float best_err = FLT_MAX; + + // Find the width/height ratio that's within 1% of the aspect ratio with a loop limit. + for (int i = 100; i; i--) { + float err = fast_fabsf(r - fast_roundf(r)); + + if (err <= best_err) { + best_err = err; + best_r = r; + best_c = c; + } + + if (best_err <= 0.01f) { + break; + } + + r += aspect_ratio; + c += 1; + } + + // Select the larger geometry to map the aspect ratio to. + int u_sub, v_sub; + + if (window_w > window_h) { + u_sub = fast_roundf(best_r); + v_sub = best_c; + } else { + u_sub = best_c; + v_sub = fast_roundf(best_r); + } + + // Crop the frame buffer while keeping the aspect ratio and keeping the width/height even. + while (((MAIN_FB()->u * MAIN_FB()->v * bpp) > size) || (MAIN_FB()->u % 2) || (MAIN_FB()->v % 2)) { + MAIN_FB()->u -= u_sub; + MAIN_FB()->v -= v_sub; + } + + // Center the new window using the previous offset and keep the offset even. + MAIN_FB()->x += (window_w - MAIN_FB()->u) / 2; + MAIN_FB()->y += (window_h - MAIN_FB()->v) / 2; + + if (MAIN_FB()->x % 2) { + MAIN_FB()->x -= 1; + } + + if (MAIN_FB()->y % 2) { + MAIN_FB()->y -= 1; + } +} + +// This is the default snapshot function, which can be replaced in sensor_init functions. +int sensor_snapshot(sensor_t *sensor, image_t *image, streaming_cb_t streaming_cb) +{ + uint8_t *b = MAIN_FB()->pixels; + uint32_t _width = MAIN_FB()->w; + uint32_t _height = MAIN_FB()->h; + int bytesPerRow = _width * 2; // Always read 2 BPP + bool _grayscale = (sensor->pixformat == PIXFORMAT_GRAYSCALE); + + uint32_t ulPin = 32; // P1.xx set of GPIO is in 'pin' 32 and above + NRF_GPIO_Type *port = nrf_gpio_pin_port_decode(&ulPin); + + // Compress the framebuffer for the IDE preview, only if it's not the first frame, + // the framebuffer is enabled and the image sensor does not support JPEG encoding. + // Note: This doesn't run unless the IDE is connected and the framebuffer is enabled. + fb_update_jpeg_buffer(); + + noInterrupts(); + + // Falling edge indicates start of frame + while ((*_vsyncPort & _vsyncMask) == 0); // wait for HIGH + while ((*_vsyncPort & _vsyncMask) != 0); // wait for LOW + + for (int i = 0; i < _height; i++) { + // rising edge indicates start of line + while ((*_hrefPort & _hrefMask) == 0); // wait for HIGH + + for (int j = 0; j < bytesPerRow; j++) { + // rising edges clock each data byte + while ((*_pclkPort & _pclkMask) != 0); // wait for LOW + + uint32_t in = port->IN; // read all bits in parallel + //in = (in >> 8) | ((in>>2) & 3); + in >>= 2; // place bits 0 and 1 at the "bottom" of the register + in &= 0x3f03; // isolate the 8 bits we care about + in |= (in >> 6); // combine the upper 6 and lower 2 bits + + if (!(j & 1) || !_grayscale) { + *b++ = in; + } + while ((*_pclkPort & _pclkMask) == 0); // wait for HIGH + } + while ((*_hrefPort & _hrefMask) != 0); // wait for LOW + } + + interrupts(); + + // Fix the BPP. + switch (sensor->pixformat) { + case PIXFORMAT_GRAYSCALE: + MAIN_FB()->bpp = 1; + break; + case PIXFORMAT_YUV422: + case PIXFORMAT_RGB565: { + MAIN_FB()->bpp = 2; + if (SENSOR_HW_FLAGS_GET(sensor, SWNSOR_HW_FLAGS_RGB565_REV)) { + unaligned_memcpy_rev16(MAIN_FB()->pixels, MAIN_FB()->pixels, _width*_height); + } + break; + } + case PIXFORMAT_BAYER: + MAIN_FB()->bpp = 3; + break; + default: + MAIN_FB()->bpp = -1; + break; + } + + // Set the user image. + if (image != NULL) { + image->w = MAIN_FB()->w; + image->h = MAIN_FB()->h; + image->bpp = MAIN_FB()->bpp; + image->pixels = MAIN_FB()->pixels; + } + return 0; +}