From a7e442c9ca52235224e0fb93e3201360a29eaa9a Mon Sep 17 00:00:00 2001 From: "Kwabena W. Agyeman" Date: Sat, 21 Apr 2018 01:45:43 -0400 Subject: [PATCH] Add FLIR Lepton Module --- src/Makefile | 1 + src/omv/Makefile | 1 + src/omv/lepton.c | 527 +++++++++++++++++++++++++++++++++++++++++++++++ src/omv/lepton.h | 13 ++ 4 files changed, 542 insertions(+) create mode 100644 src/omv/lepton.c create mode 100644 src/omv/lepton.h diff --git a/src/Makefile b/src/Makefile index 99ff4ef1c..b1965a30e 100755 --- a/src/Makefile +++ b/src/Makefile @@ -148,6 +148,7 @@ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/, \ ov2640.o \ ov7725.o \ mt9v034.o \ + lepton.o \ sensor.o \ stm32fxxx_hal_msp.o \ soft_i2c.o \ diff --git a/src/omv/Makefile b/src/omv/Makefile index 8c3a74b6a..d7a9fad8d 100644 --- a/src/omv/Makefile +++ b/src/omv/Makefile @@ -15,6 +15,7 @@ SRCS += $(addprefix , \ ov2640.c \ ov7725.c \ mt9v034.c \ + lepton.c \ sensor.c \ stm32fxxx_hal_msp.c \ soft_i2c.c \ diff --git a/src/omv/lepton.c b/src/omv/lepton.c new file mode 100644 index 000000000..3dd7b995b --- /dev/null +++ b/src/omv/lepton.c @@ -0,0 +1,527 @@ +/* + * This file is part of the OpenMV project. + * Copyright (c) 2013-2018 Ibrahim Abdelkader + * This work is licensed under the MIT license, see the file LICENSE for details. + * + * Lepton driver. + * + */ + +#include STM32_HAL_H +#include "cambus.h" +#include "sensor.h" +#include "systick.h" +#include "framebuffer.h" +#include "omv_boardconfig.h" + +#include "crc16.h" +#include "LEPTON_SDK.h" +#include "LEPTON_AGC.h" +#include "LEPTON_SYS.h" +#include "LEPTON_VID.h" +#include "LEPTON_OEM.h" +#include "LEPTON_RAD.h" +#include "LEPTON_I2C_Reg.h" + +#define VOSPI_LINE_PIXELS (80) +#define VOSPI_NUMBER_PACKETS (60) +#define VOSPI_SPECIAL_LINE (20) + +static int h_res = 0; +static int v_res = 0; +static bool h_mirror = false; +static bool v_flip = false; + +static SPI_HandleTypeDef SPIHandle; +static LEP_CAMERA_PORT_DESC_T handle; +extern const uint16_t rainbow_table[256]; + +static int reset(sensor_t *sensor) +{ + memset(&handle, 0, sizeof(handle)); + + h_res = 0; + v_res = 0; + h_mirror = false; + v_flip = false; + + DCMI_PWDN_LOW(); + systick_sleep(10); + + DCMI_PWDN_HIGH(); + systick_sleep(10); + + DCMI_RESET_LOW(); + systick_sleep(10); + + DCMI_RESET_HIGH(); + systick_sleep(1000); + + LEP_CAMERA_PORT_DESC_T tmp_handle; + + bool okay = false; + + for (int i = 0; i < 1000; i++) { + LEP_RESULT result = LEP_OpenPort(0, LEP_CCI_TWI, 0, &tmp_handle); + + if (result == LEP_OK) { + okay = true; + break; + } else { + systick_sleep(1); + } + } + + if (!okay) { + return -1; + } + + bool booted = false; + + for (int i = 0; i < 1000; i++) { + LEP_SDK_BOOT_STATUS_E status; + if (LEP_GetCameraBootStatus(&tmp_handle, &status) != LEP_OK) { + return -1; + } + + if (status == LEP_BOOT_STATUS_BOOTED) { + booted = true; + break; + } else { + systick_sleep(1); + } + } + + if (!booted) { + return -1; + } + + bool busy = true; + + for (int i = 0; i < 1000; i++) { + LEP_UINT16 status; + if (LEP_DirectReadRegister(&tmp_handle, LEP_I2C_STATUS_REG, &status) != LEP_OK) { + return -1; + } + + if (!(status & LEP_I2C_STATUS_BUSY_BIT_MASK)) { + busy = false; + break; + } else { + systick_sleep(1); + } + } + + if (busy) { + return -1; + } + + busy = true; + + for (int i = 0; i < 5000; i++) { + LEP_SYS_STATUS_E status; + if (LEP_GetSysFFCStatus(&tmp_handle, &status) != LEP_OK) { + return -1; + } + + if (status == LEP_SYS_STATUS_READY) { + busy = false; + break; + } else { + systick_sleep(1); + } + } + + if (busy) { + return -1; + } + + if (LEP_SetRadEnableState(&tmp_handle, LEP_RAD_DISABLE) != LEP_OK) { + return -1; + } + + LEP_AGC_ROI_T roi; + if (LEP_GetAgcROI(&tmp_handle, &roi) != LEP_OK) { + return -1; + } + + int tmp_h_res = roi.endCol + 1; + int tmp_v_res = roi.endRow + 1; + + if (LEP_SetAgcEnableState(&tmp_handle, LEP_AGC_ENABLE) != LEP_OK) { + return -1; + } + if (LEP_SetAgcCalcEnableState(&tmp_handle, LEP_AGC_ENABLE) != LEP_OK) { + return -1; + } + + handle = tmp_handle; + h_res = tmp_h_res; + v_res = tmp_v_res; + return 0; +} + +static int sleep(sensor_t *sensor, int enable) +{ + if (enable) { + DCMI_PWDN_LOW(); + systick_sleep(100); + } else { + DCMI_PWDN_HIGH(); + systick_sleep(100); + } + + return 0; +} + +static int read_reg(sensor_t *sensor, uint8_t reg_addr) +{ + uint16_t reg_data; + if (cambus_readw2(sensor->slv_addr, reg_addr, ®_data)) { + return -1; + } + return reg_data; +} + +static int write_reg(sensor_t *sensor, uint8_t reg_addr, uint16_t reg_data) +{ + return cambus_writew2(sensor->slv_addr, reg_addr, reg_data); +} + +static int set_pixformat(sensor_t *sensor, pixformat_t pixformat) +{ + return 0; +} + +static int set_framesize(sensor_t *sensor, framesize_t framesize) +{ + return 0; +} + +static int set_framerate(sensor_t *sensor, framerate_t framerate) +{ + return 0; +} + +static int set_contrast(sensor_t *sensor, int level) +{ + return 0; +} + +static int set_brightness(sensor_t *sensor, int level) +{ + return 0; +} + +static int set_saturation(sensor_t *sensor, int level) +{ + return 0; +} + +static int set_gainceiling(sensor_t *sensor, gainceiling_t gainceiling) +{ + return 0; +} + +static int set_quality(sensor_t *sensor, int quality) +{ + return 0; +} + +static int set_colorbar(sensor_t *sensor, int enable) +{ + return 0; +} + +static int set_special_effect(sensor_t *sensor, sde_t sde) +{ + return 0; +} + +static int set_auto_gain(sensor_t *sensor, int enable, float gain_db, float gain_db_ceiling) +{ + return 0; +} + +static int get_gain_db(sensor_t *sensor, float *gain_db) +{ + return 0; +} + +static int set_auto_exposure(sensor_t *sensor, int enable, int exposure_us) +{ + return 0; +} + +static int get_exposure_us(sensor_t *sensor, int *exposure_us) +{ + return 0; +} + +static int set_auto_whitebal(sensor_t *sensor, int enable, float r_gain_db, float g_gain_db, float b_gain_db) +{ + return 0; +} + +static int get_rgb_gain_db(sensor_t *sensor, float *r_gain_db, float *g_gain_db, float *b_gain_db) +{ + return 0; +} + +static int set_hmirror(sensor_t *sensor, int enable) +{ + h_mirror = enable; + return 0; +} + +static int set_vflip(sensor_t *sensor, int enable) +{ + v_flip = enable; + return 0; +} + +static int set_lens_correction(sensor_t *sensor, int enable, int radi, int coef) +{ + return 0; +} + +static int snapshot(sensor_t *sensor, image_t *image, line_filter_t line_filter_func, void *line_filter_args) +{ + if ((!h_res) || (!v_res) || (!sensor->framesize) || (!sensor->pixformat)) { + return -1; + } + + int y_scaler = h_res / VOSPI_LINE_PIXELS; + int vospi_line_size = sizeof(uint16_t) + sizeof(uint16_t) + (VOSPI_LINE_PIXELS * sizeof(uint16_t)); + uint8_t buffer[vospi_line_size]; + + bool reset = false; + uint32_t time = systick_current_millis(); + for (int y = 0; y < v_res;) { + for (int x = 0; x < h_res;) { + if (sys_tick_has_passed(time, (h_res > VOSPI_LINE_PIXELS) ? 3000 : 1000)) { + return -1; + } + + int state = __get_PRIMASK(); + __disable_irq(); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_RESET); + HAL_StatusTypeDef status = HAL_SPI_Receive(&SPIHandle, buffer, vospi_line_size, 1000); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_SET); + __set_PRIMASK(state); + + if (status != HAL_OK) { + return -1; + } + + if ((buffer[0] & 0xF) == 0xF) { + continue; + } + + int ttt = (buffer[0] >> 4) & 0x7; + int packet_num = ((buffer[0] << 8) | (buffer[1] << 0)) & 0xFFF; + int crc = (buffer[2] << 8) | (buffer[3] << 0); + + buffer[0] &= 0x0F; + buffer[1] &= 0xFF; + buffer[2] = 0; + buffer[3] = 0; + + if (CalcCRC16Bytes(vospi_line_size, (char *) buffer) != crc) { + systick_sleep(200); + y = x = 0; + reset = false; + continue; + } + + if ((y == 0) && (x == 0) && (packet_num != 0)) { + systick_sleep(200); + y = x = 0; + reset = false; + continue; + } + + if ((((y % (VOSPI_NUMBER_PACKETS / y_scaler)) * y_scaler) + (x / VOSPI_LINE_PIXELS)) != packet_num) { + systick_sleep(200); + y = x = 0; + reset = false; + continue; + } + + if ((h_res > VOSPI_LINE_PIXELS) + && (packet_num == VOSPI_SPECIAL_LINE) + && (((y / (VOSPI_NUMBER_PACKETS / y_scaler)) + 1) != ttt)) { + if (!ttt) { + reset = true; + } else { + systick_sleep(200); + y = x = 0; + reset = false; + continue; + } + } + + //image_t img; + //img.w = sensor->fb_w; + //img.h = sensor->fb_h; + //img.bpp = MAIN_FB()->bpp; // invalid + //img.data = MAIN_FB()->pixels; // valid + + //float x_scale = resolution[sensor->framesize][0] / ((float) h_res); + //float y_scale = resolution[sensor->framesize][1] / ((float) v_res); + // MAX == KeepAspectRationByExpanding - MIN == KeepAspectRatio + //float scale = IM_MAX(x_scale, y_scale); + //int x_offset = (resolution[sensor->framesize][0] - (h_res * scale)) / 2; + //int y_offset = (resolution[sensor->framesize][1] - (v_res * scale)) / 2; + //// The code below upscales the source image to the requested frame size + //// and then crops it to the window set by the user. + + //for (int yyy = fast_floorf(y * scale) + y_offset, + // yyyy = fast_ceilf((y + 1) * scale) + y_offset; yyy < yyyy; yyy++) { + // if ((sensor->fb_y <= yyy) && (yyy < (sensor->fb_y + sensor->fb_h))) { + + // for (int xxx = fast_floorf(x * scale) + x_offset, + // xxxx = fast_ceilf((x + VOSPI_LINE_PIXELS) * scale) + x_offset; xxx < xxxx; xxx++) { + // if ((sensor->fb_x <= xxx) && (xxx < (sensor->fb_x + sensor->fb_w))) { + + // int i = (xxx / scale) - x; + // // Value is the 14-bit value from the FLIR IR camera. + // // However, with AGC enabled only the bottom 8-bits are non-zero. + // int value = ((buffer[(i*2)+4] << 8) | (buffer[(i*2)+5] << 0)) & 0x3FFF; + + // int t_x = xxx - sensor->fb_x; + // int t_y = yyy - sensor->fb_y; + + // if (h_mirror) t_x = sensor->fb_w - t_x - 1; + // if (v_flip) t_y = sensor->fb_h - t_y - 1; + + // switch (sensor->pixformat) { + // case PIXFORMAT_RGB565: { + // IMAGE_PUT_RGB565_PIXEL(&img, t_x, t_y, rainbow_table[value & 0xFF]); + // break; + // } + // case PIXFORMAT_GRAYSCALE: { + // IMAGE_PUT_GRAYSCALE_PIXEL(&img, t_x, t_y, value & 0xFF); + // break; + // } + // default: { + // break; + // } + // } + // } + // } + // } + //} + + x += VOSPI_LINE_PIXELS; + } + + y += 1; + + if (reset && (!(y % (VOSPI_NUMBER_PACKETS / y_scaler)))) { + y -= VOSPI_NUMBER_PACKETS / y_scaler; + reset = false; + } + } + + return 0; +} + +int lepton_init(sensor_t *sensor) +{ + memset(&SPIHandle, 0, sizeof(SPIHandle)); + + SPIHandle.Instance = SPI3; + SPIHandle.Init.Mode = SPI_MODE_MASTER; + SPIHandle.Init.Direction = SPI_DIRECTION_2LINES_RXONLY; + SPIHandle.Init.DataSize = SPI_DATASIZE_8BIT; + SPIHandle.Init.CLKPolarity = SPI_POLARITY_HIGH; + SPIHandle.Init.CLKPhase = SPI_PHASE_2EDGE; + SPIHandle.Init.NSS = SPI_NSS_SOFT; + + uint32_t prescale = HAL_RCC_GetPCLK1Freq() / 20000000; + if (prescale <= 2) { SPIHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2; } + else if (prescale <= 4) { SPIHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4; } + else if (prescale <= 8) { SPIHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; } + else if (prescale <= 16) { SPIHandle.Init.BaudRatePrescaler= SPI_BAUDRATEPRESCALER_16; } + else if (prescale <= 32) { SPIHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; } + else if (prescale <= 64) { SPIHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64; } + else if (prescale <= 128) { SPIHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; } + else { SPIHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256; } + + __HAL_RCC_SPI3_CLK_ENABLE(); + if (HAL_SPI_Init(&SPIHandle) != HAL_OK) { + __HAL_RCC_SPI3_FORCE_RESET(); + __HAL_RCC_SPI3_RELEASE_RESET(); + __HAL_RCC_SPI3_CLK_DISABLE(); + return -1; + } + + GPIO_InitTypeDef GPIO_InitTypeDefSS; + GPIO_InitTypeDefSS.Pin = GPIO_PIN_15; + GPIO_InitTypeDefSS.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitTypeDefSS.Pull = GPIO_NOPULL; + GPIO_InitTypeDefSS.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitTypeDefSS.Alternate = GPIO_AF6_SPI3; + HAL_GPIO_Init(GPIOA, &GPIO_InitTypeDefSS); + + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_SET); + + GPIO_InitTypeDef GPIO_InitTypeDefSCLK; + GPIO_InitTypeDefSCLK.Pin = GPIO_PIN_3; + GPIO_InitTypeDefSCLK.Mode = GPIO_MODE_AF_PP; + GPIO_InitTypeDefSCLK.Pull = GPIO_NOPULL; + GPIO_InitTypeDefSCLK.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitTypeDefSCLK.Alternate = GPIO_AF6_SPI3; + HAL_GPIO_Init(GPIOB, &GPIO_InitTypeDefSCLK); + + GPIO_InitTypeDef GPIO_InitTypeDefMISO; + GPIO_InitTypeDefMISO.Pin = GPIO_PIN_4; + GPIO_InitTypeDefMISO.Mode = GPIO_MODE_AF_PP; + GPIO_InitTypeDefMISO.Pull = GPIO_NOPULL; + GPIO_InitTypeDefMISO.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitTypeDefMISO.Alternate = GPIO_AF6_SPI3; + HAL_GPIO_Init(GPIOB, &GPIO_InitTypeDefMISO); + + GPIO_InitTypeDef GPIO_InitTypeDefMOSI; + GPIO_InitTypeDefMOSI.Pin = GPIO_PIN_5; + GPIO_InitTypeDefMOSI.Mode = GPIO_MODE_AF_PP; + GPIO_InitTypeDefMOSI.Pull = GPIO_NOPULL; + GPIO_InitTypeDefMOSI.Speed = GPIO_SPEED_FREQ_HIGH; + GPIO_InitTypeDefMOSI.Alternate = GPIO_AF7_SPI3; + HAL_GPIO_Init(GPIOB, &GPIO_InitTypeDefMOSI); + + sensor->gs_bpp = sizeof(uint8_t); + sensor->reset = reset; + sensor->sleep = sleep; + sensor->snapshot = snapshot; + sensor->read_reg = read_reg; + sensor->write_reg = write_reg; + sensor->set_pixformat = set_pixformat; + sensor->set_framesize = set_framesize; + sensor->set_framerate = set_framerate; + sensor->set_contrast = set_contrast; + sensor->set_brightness = set_brightness; + sensor->set_saturation = set_saturation; + sensor->set_gainceiling = set_gainceiling; + sensor->set_quality = set_quality; + sensor->set_colorbar = set_colorbar; + sensor->set_special_effect = set_special_effect; + sensor->set_auto_gain = set_auto_gain; + sensor->get_gain_db = get_gain_db; + sensor->set_auto_exposure = set_auto_exposure; + sensor->get_exposure_us = get_exposure_us; + sensor->set_auto_whitebal = set_auto_whitebal; + sensor->get_rgb_gain_db = get_rgb_gain_db; + sensor->set_hmirror = set_hmirror; + sensor->set_vflip = set_vflip; + sensor->set_lens_correction = set_lens_correction; + + SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_VSYNC, 1); + SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_HSYNC, 0); + SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_PIXCK, 0); + SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_FSYNC, 0); + SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_JPEGE, 0); + + return 0; +} diff --git a/src/omv/lepton.h b/src/omv/lepton.h new file mode 100644 index 000000000..3e4ec84f1 --- /dev/null +++ b/src/omv/lepton.h @@ -0,0 +1,13 @@ +/* + * This file is part of the OpenMV project. + * Copyright (c) 2013-2018 Ibrahim Abdelkader + * This work is licensed under the MIT license, see the file LICENSE for details. + * + * Lepton driver. + * + */ +#ifndef __LEPTON_H__ +#define __LEPTON_H__ +#include "sensor.h" +int lepton_init(sensor_t *sensor); +#endif // __LEPTON_H__