mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
sensors/lepton: Use new VOSPI/SPI HAL.
* This change makes lepton sensor driver completely generic, by moving all of the VOSPI code to vospi.c, which in turn uses the new OMV SPI HAL.
This commit is contained in:
parent
0a65ce121c
commit
f68f6d70e4
@ -26,6 +26,7 @@ SRCS += $(addprefix common/, \
|
||||
tinyusb_debug.c \
|
||||
sensor_utils.c \
|
||||
factoryreset.c \
|
||||
vospi.c \
|
||||
)
|
||||
|
||||
SRCS += $(addprefix sensors/, \
|
||||
|
||||
@ -62,7 +62,7 @@
|
||||
#define OMV_ENABLE_MT9V0XX (1)
|
||||
#define OMV_ENABLE_LEPTON (1)
|
||||
#define OMV_ENABLE_HM01B0 (0)
|
||||
#define OMV_ENABLE_PAJ6100 (1)
|
||||
#define OMV_ENABLE_PAJ6100 (0)
|
||||
#define OMV_ENABLE_FROGEYE2020 (1)
|
||||
#define OMV_ENABLE_FIR_MLX90621 (1)
|
||||
#define OMV_ENABLE_FIR_MLX90640 (1)
|
||||
@ -216,7 +216,7 @@
|
||||
#define OMV_AXI_QOS_MDMA_R_PRI 15 // Max pri to move data.
|
||||
#define OMV_AXI_QOS_MDMA_W_PRI 15 // Max pri to move data.
|
||||
|
||||
// Image sensor I2C
|
||||
// Main image sensor I2C bus
|
||||
#define ISC_I2C (I2C1)
|
||||
#define ISC_I2C_ID (1)
|
||||
#define ISC_I2C_SPEED (OMV_I2C_SPEED_STANDARD)
|
||||
@ -227,7 +227,7 @@
|
||||
#define ISC_I2C_FORCE_RESET() __HAL_RCC_I2C1_FORCE_RESET()
|
||||
#define ISC_I2C_RELEASE_RESET() __HAL_RCC_I2C1_RELEASE_RESET()
|
||||
|
||||
// FIR I2C configuration.
|
||||
// Thermal image sensor I2C bus
|
||||
#define FIR_I2C (I2C2)
|
||||
#define FIR_I2C_ID (2)
|
||||
#define FIR_I2C_SPEED (OMV_I2C_SPEED_FULL)
|
||||
@ -238,11 +238,15 @@
|
||||
#define FIR_I2C_FORCE_RESET() __HAL_RCC_I2C2_FORCE_RESET()
|
||||
#define FIR_I2C_RELEASE_RESET() __HAL_RCC_I2C2_RELEASE_RESET()
|
||||
|
||||
// Soft I2C bus.
|
||||
// Soft I2C bus
|
||||
#define SOFT_I2C_SIOC_PIN (&omv_pin_B10_GPIO)
|
||||
#define SOFT_I2C_SIOD_PIN (&omv_pin_B11_GPIO)
|
||||
#define SOFT_I2C_SPIN_DELAY 64
|
||||
|
||||
// Main image sensor SPI bus
|
||||
#define ISC_SPI_ID (3)
|
||||
#define ISC_SPI_BAUDRATE (20000000)
|
||||
|
||||
// DCMI timer.
|
||||
#define DCMI_TIM (TIM1)
|
||||
#define DCMI_TIM_PIN (&omv_pin_A8_TIM1)
|
||||
@ -269,6 +273,15 @@
|
||||
#define DCMI_VSYNC_PIN (&omv_pin_B7_DCMI)
|
||||
#define DCMI_PXCLK_PIN (&omv_pin_A6_DCMI)
|
||||
|
||||
// Physical SPI buses.
|
||||
#define SPI3_ID (3)
|
||||
#define SPI3_SCLK_PIN (&omv_pin_B3_SPI3)
|
||||
#define SPI3_MISO_PIN (&omv_pin_B4_SPI3)
|
||||
#define SPI3_MOSI_PIN (&omv_pin_B5_SPI3)
|
||||
#define SPI3_SSEL_PIN (&omv_pin_A15_SPI3)
|
||||
#define SPI3_DMA_TX_CHANNEL (DMA1_Stream7)
|
||||
#define SPI3_DMA_RX_CHANNEL (DMA1_Stream2)
|
||||
|
||||
#define WINC_SPI (SPI2)
|
||||
#define WINC_SPI_TIMEOUT (1000)
|
||||
// SPI1/2/3 clock source is PLL2 (160MHz/4 == 40MHz).
|
||||
@ -284,30 +297,6 @@
|
||||
#define WINC_RST_PIN (&omv_pin_D12_GPIO)
|
||||
#define WINC_IRQ_PIN (&omv_pin_D13_GPIO)
|
||||
|
||||
#define ISC_SPI (SPI3)
|
||||
// SPI1/2/3 clock source is PLL3 (160MHz/8 == 20MHz) - Minimum (164*240*8*27 = 8,501,760Hz)
|
||||
#define ISC_SPI_PRESCALER (SPI_BAUDRATEPRESCALER_8)
|
||||
|
||||
#define ISC_SPI_IRQn (SPI3_IRQn)
|
||||
#define ISC_SPI_IRQHandler (SPI3_IRQHandler)
|
||||
|
||||
#define ISC_SPI_DMA_IRQn (DMA1_Stream0_IRQn)
|
||||
#define ISC_SPI_DMA_STREAM (DMA1_Stream0)
|
||||
|
||||
#define ISC_SPI_DMA_REQUEST (DMA_REQUEST_SPI3_RX)
|
||||
#define ISC_SPI_DMA_IRQHandler (DMA1_Stream0_IRQHandler)
|
||||
|
||||
#define ISC_SPI_RESET() __HAL_RCC_SPI3_FORCE_RESET()
|
||||
#define ISC_SPI_RELEASE() __HAL_RCC_SPI3_RELEASE_RESET()
|
||||
|
||||
#define ISC_SPI_CLK_ENABLE() __HAL_RCC_SPI3_CLK_ENABLE()
|
||||
#define ISC_SPI_CLK_DISABLE() __HAL_RCC_SPI3_CLK_DISABLE()
|
||||
|
||||
#define ISC_SPI_SCLK_PIN (&omv_pin_B3_SPI3)
|
||||
#define ISC_SPI_MISO_PIN (&omv_pin_B4_SPI3)
|
||||
#define ISC_SPI_MOSI_PIN (&omv_pin_B5_SPI3)
|
||||
#define ISC_SPI_SSEL_PIN (&omv_pin_A15_SPI3)
|
||||
|
||||
// SPI LCD Interface
|
||||
#define OMV_SPI_LCD_CONTROLLER (&spi_obj[1])
|
||||
#define OMV_SPI_LCD_CONTROLLER_INSTANCE (SPI2)
|
||||
|
||||
@ -61,7 +61,7 @@
|
||||
#define OMV_ENABLE_MT9V0XX (1)
|
||||
#define OMV_ENABLE_LEPTON (1)
|
||||
#define OMV_ENABLE_HM01B0 (0)
|
||||
#define OMV_ENABLE_PAJ6100 (1)
|
||||
#define OMV_ENABLE_PAJ6100 (0)
|
||||
#define OMV_ENABLE_FROGEYE2020 (1)
|
||||
#define OMV_ENABLE_FIR_MLX90621 (1)
|
||||
#define OMV_ENABLE_FIR_MLX90640 (1)
|
||||
@ -219,7 +219,7 @@
|
||||
#define OMV_AXI_QOS_MDMA_R_PRI 15 // Max pri to move data.
|
||||
#define OMV_AXI_QOS_MDMA_W_PRI 15 // Max pri to move data.
|
||||
|
||||
// Image sensor I2C.
|
||||
// Main image sensor I2C bus
|
||||
#define ISC_I2C (I2C1)
|
||||
#define ISC_I2C_ID (1)
|
||||
#define ISC_I2C_SPEED (OMV_I2C_SPEED_STANDARD)
|
||||
@ -230,7 +230,7 @@
|
||||
#define ISC_I2C_FORCE_RESET() __HAL_RCC_I2C1_FORCE_RESET()
|
||||
#define ISC_I2C_RELEASE_RESET() __HAL_RCC_I2C1_RELEASE_RESET()
|
||||
|
||||
// FIR I2C configuration.
|
||||
// Thermal image sensor I2C bus
|
||||
#define FIR_I2C (I2C2)
|
||||
#define FIR_I2C_ID (2)
|
||||
#define FIR_I2C_SPEED (OMV_I2C_SPEED_FULL)
|
||||
@ -246,6 +246,10 @@
|
||||
#define SOFT_I2C_SIOD_PIN (&omv_pin_B11_GPIO)
|
||||
#define SOFT_I2C_SPIN_DELAY 64
|
||||
|
||||
// Main image sensor SPI bus
|
||||
#define ISC_SPI_ID (3)
|
||||
#define ISC_SPI_BAUDRATE (20000000)
|
||||
|
||||
// DCMI timer.
|
||||
#define DCMI_TIM (TIM1)
|
||||
#define DCMI_TIM_PIN (&omv_pin_A8_TIM1)
|
||||
@ -272,6 +276,15 @@
|
||||
#define DCMI_VSYNC_PIN (&omv_pin_B7_DCMI)
|
||||
#define DCMI_PXCLK_PIN (&omv_pin_A6_DCMI)
|
||||
|
||||
// Physical SPI buses.
|
||||
#define SPI3_ID (3)
|
||||
#define SPI3_SCLK_PIN (&omv_pin_B3_SPI3)
|
||||
#define SPI3_MISO_PIN (&omv_pin_B4_SPI3)
|
||||
#define SPI3_MOSI_PIN (&omv_pin_B5_SPI3)
|
||||
#define SPI3_SSEL_PIN (&omv_pin_A15_SPI3)
|
||||
#define SPI3_DMA_TX_CHANNEL (DMA1_Stream7)
|
||||
#define SPI3_DMA_RX_CHANNEL (DMA1_Stream2)
|
||||
|
||||
#define WINC_SPI (SPI2)
|
||||
#define WINC_SPI_TIMEOUT (1000)
|
||||
// SPI1/2/3 clock source is PLL2 (160MHz/4 == 40MHz).
|
||||
@ -286,30 +299,6 @@
|
||||
#define WINC_RST_PIN (&omv_pin_D12_GPIO)
|
||||
#define WINC_IRQ_PIN (&omv_pin_D13_GPIO)
|
||||
|
||||
#define ISC_SPI (SPI3)
|
||||
// SPI1/2/3 clock source is PLL3 (160MHz/8 == 20MHz) - Minimum (164*240*8*27 = 8,501,760Hz)
|
||||
#define ISC_SPI_PRESCALER (SPI_BAUDRATEPRESCALER_8)
|
||||
|
||||
#define ISC_SPI_IRQn (SPI3_IRQn)
|
||||
#define ISC_SPI_IRQHandler (SPI3_IRQHandler)
|
||||
|
||||
#define ISC_SPI_DMA_IRQn (DMA1_Stream0_IRQn)
|
||||
#define ISC_SPI_DMA_STREAM (DMA1_Stream0)
|
||||
|
||||
#define ISC_SPI_DMA_REQUEST (DMA_REQUEST_SPI3_RX)
|
||||
#define ISC_SPI_DMA_IRQHandler (DMA1_Stream0_IRQHandler)
|
||||
|
||||
#define ISC_SPI_RESET() __HAL_RCC_SPI3_FORCE_RESET()
|
||||
#define ISC_SPI_RELEASE() __HAL_RCC_SPI3_RELEASE_RESET()
|
||||
|
||||
#define ISC_SPI_CLK_ENABLE() __HAL_RCC_SPI3_CLK_ENABLE()
|
||||
#define ISC_SPI_CLK_DISABLE() __HAL_RCC_SPI3_CLK_DISABLE()
|
||||
|
||||
#define ISC_SPI_SCLK_PIN (&omv_pin_B3_SPI3)
|
||||
#define ISC_SPI_MISO_PIN (&omv_pin_B4_SPI3)
|
||||
#define ISC_SPI_MOSI_PIN (&omv_pin_B5_SPI3)
|
||||
#define ISC_SPI_SSEL_PIN (&omv_pin_A15_SPI3)
|
||||
|
||||
// SPI LCD Interface
|
||||
#define OMV_SPI_LCD_CONTROLLER (&spi_obj[1])
|
||||
#define OMV_SPI_LCD_CONTROLLER_INSTANCE (SPI2)
|
||||
|
||||
168
src/omv/common/vospi.c
Normal file
168
src/omv/common/vospi.c
Normal file
@ -0,0 +1,168 @@
|
||||
/*
|
||||
* This file is part of the OpenMV project.
|
||||
*
|
||||
* Copyright (c) 2023 Ibrahim Abdelkader <iabdalkader@openmv.io>
|
||||
* Copyright (c) 2023 Kwabena W. Agyeman <kwagyeman@openmv.io>
|
||||
*
|
||||
* This work is licensed under the MIT license, see the file LICENSE for details.
|
||||
*
|
||||
* VOSPI driver.
|
||||
*/
|
||||
#include "omv_boardconfig.h"
|
||||
#if OMV_ENABLE_VOSPI || OMV_ENABLE_LEPTON
|
||||
|
||||
#include <stdint.h>
|
||||
#include "py/mphal.h"
|
||||
|
||||
#include "vospi.h"
|
||||
#include "crc16.h"
|
||||
#include "common.h"
|
||||
#include "omv_spi.h"
|
||||
|
||||
#define VOSPI_LINE_PIXELS (80)
|
||||
#define VOSPI_NUMBER_PACKETS (60)
|
||||
#define VOSPI_SPECIAL_PACKET (20)
|
||||
#define VOSPI_LINE_SIZE (80 * 2)
|
||||
#define VOSPI_HEADER_SIZE (4)
|
||||
#define VOSPI_PACKET_SIZE (VOSPI_HEADER_SIZE + VOSPI_LINE_SIZE)
|
||||
#define VOSPI_FIRST_PACKET (0)
|
||||
#define VOSPI_FIRST_SEGMENT (1)
|
||||
#ifndef VOSPI_PACKET_ALIGNMENT
|
||||
#define VOSPI_PACKET_ALIGNMENT (4)
|
||||
#endif
|
||||
#define VOSPI_HEADER_SEG(buf) (((buf[0] >> 4) & 0x7))
|
||||
#define VOSPI_HEADER_PID(buf) (((buf[0] << 8) | (buf[1] << 0)) & 0x0FFF)
|
||||
#define VOSPI_HEADER_CRC(buf) (((buf[2] << 8) | (buf[3] << 0)))
|
||||
|
||||
typedef enum {
|
||||
VOSPI_FLAGS_RESET = (1 << 0),
|
||||
VOSPI_FLAGS_RESYNC = (1 << 1),
|
||||
} vospi_flags_t;
|
||||
|
||||
typedef struct _vospi_state {
|
||||
volatile uint32_t pid;
|
||||
volatile uint32_t sid;
|
||||
uint8_t *buffer;
|
||||
uint32_t n_packets;
|
||||
omv_spi_t spi_bus;
|
||||
volatile uint32_t flags;
|
||||
} vospi_state_t;
|
||||
|
||||
static vospi_state_t vospi;
|
||||
static uint8_t OMV_ATTR_SECTION(
|
||||
OMV_ATTR_ALIGNED(vospi_packet[VOSPI_PACKET_SIZE], VOSPI_PACKET_ALIGNMENT), ".dma_buffer"
|
||||
);
|
||||
|
||||
static void vospi_callback(omv_spi_t *spi, void *data);
|
||||
#if (OMV_ENABLE_VOSPI_CRC)
|
||||
static uint16_t vospi_calc_crc(uint8_t *buf)
|
||||
{
|
||||
buf[0] &= 0x0F;
|
||||
buf[1] &= 0xFF;
|
||||
buf[2] = 0;
|
||||
buf[3] = 0;
|
||||
return CalcCRC16Bytes(VOSPI_PACKET_SIZE, (char *) buf);
|
||||
}
|
||||
#endif
|
||||
|
||||
static void vospi_do_resync()
|
||||
{
|
||||
omv_spi_transfer_t spi_xfer = {
|
||||
.txbuf = NULL,
|
||||
.rxbuf = vospi_packet,
|
||||
.size = VOSPI_PACKET_SIZE,
|
||||
.timeout = 0,
|
||||
.flags = OMV_SPI_XFER_DMA | OMV_SPI_XFER_CIRCULAR,
|
||||
.userdata = NULL,
|
||||
.callback = vospi_callback,
|
||||
};
|
||||
|
||||
omv_spi_transfer_abort(&vospi.spi_bus);
|
||||
mp_hal_delay_ms(200);
|
||||
vospi.flags = VOSPI_FLAGS_RESET;
|
||||
omv_spi_transfer_start(&vospi.spi_bus, &spi_xfer);
|
||||
debug_printf("vospi resync...\n");
|
||||
}
|
||||
|
||||
static void vospi_callback(omv_spi_t *spi, void *data)
|
||||
{
|
||||
if (vospi.flags & VOSPI_FLAGS_RESYNC) {
|
||||
// Captured a packet before an resync is complete.
|
||||
return;
|
||||
}
|
||||
|
||||
if (vospi.flags & VOSPI_FLAGS_RESET) {
|
||||
vospi.pid = VOSPI_FIRST_PACKET;
|
||||
vospi.sid = VOSPI_FIRST_SEGMENT;
|
||||
vospi.flags &= ~(VOSPI_FLAGS_RESET);
|
||||
}
|
||||
|
||||
if (vospi.pid < vospi.n_packets && (vospi_packet[0] & 0xF) != 0xF) {
|
||||
uint32_t pid = VOSPI_HEADER_PID(vospi_packet);
|
||||
uint32_t sid = VOSPI_HEADER_SEG(vospi_packet);
|
||||
if (pid != (vospi.pid % VOSPI_NUMBER_PACKETS)) {
|
||||
if (vospi.pid != VOSPI_FIRST_PACKET) {
|
||||
vospi.flags |= VOSPI_FLAGS_RESYNC; // lost sync
|
||||
debug_printf("lost sync, packet id:%lu expected id:%lu \n", pid, vospi.pid);
|
||||
}
|
||||
} else if (vospi.n_packets > VOSPI_NUMBER_PACKETS
|
||||
&& pid == VOSPI_SPECIAL_PACKET && sid != vospi.sid ) {
|
||||
if (vospi.sid != VOSPI_FIRST_SEGMENT) {
|
||||
vospi.flags |= VOSPI_FLAGS_RESYNC; // lost sync
|
||||
debug_printf("lost sync, segment id:%lu expected id:%lu\n", sid, vospi.sid);
|
||||
}
|
||||
} else {
|
||||
memcpy(vospi.buffer + vospi.pid * VOSPI_LINE_SIZE,
|
||||
vospi_packet + VOSPI_HEADER_SIZE, VOSPI_LINE_SIZE);
|
||||
if ((++vospi.pid % VOSPI_NUMBER_PACKETS) == 0) {
|
||||
vospi.sid++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int vospi_init(uint32_t n_packets, void *buffer)
|
||||
{
|
||||
memset(&vospi, 0, sizeof(vospi_state_t));
|
||||
vospi.buffer = buffer;
|
||||
vospi.n_packets = n_packets;
|
||||
// resync on first snapshot.
|
||||
vospi.flags = VOSPI_FLAGS_RESYNC;
|
||||
|
||||
omv_spi_config_t spi_config;
|
||||
omv_spi_default_config(&spi_config, ISC_SPI_ID);
|
||||
|
||||
spi_config.bus_mode = OMV_SPI_BUS_RX;
|
||||
spi_config.baudrate = ISC_SPI_BAUDRATE;
|
||||
spi_config.dma_enable = true;
|
||||
|
||||
if (omv_spi_init(&vospi.spi_bus, &spi_config) != 0) {
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int vospi_snapshot(uint32_t timeout_ms)
|
||||
{
|
||||
// Restart counters to capture a new frame.
|
||||
vospi.flags |= VOSPI_FLAGS_RESET;
|
||||
|
||||
// Snapshot start tick
|
||||
mp_uint_t tick_start = mp_hal_ticks_ms();
|
||||
|
||||
do {
|
||||
if (vospi.flags & VOSPI_FLAGS_RESYNC) {
|
||||
vospi_do_resync();
|
||||
}
|
||||
|
||||
if ((mp_hal_ticks_ms() - tick_start) >= timeout_ms) {
|
||||
// Timeout error.
|
||||
return -1;
|
||||
}
|
||||
|
||||
MICROPY_EVENT_POLL_HOOK
|
||||
} while (vospi.pid < vospi.n_packets);
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
16
src/omv/common/vospi.h
Normal file
16
src/omv/common/vospi.h
Normal file
@ -0,0 +1,16 @@
|
||||
/*
|
||||
* This file is part of the OpenMV project.
|
||||
*
|
||||
* Copyright (c) 2023 Ibrahim Abdelkader <iabdalkader@openmv.io>
|
||||
* Copyright (c) 2023 Kwabena W. Agyeman <kwagyeman@openmv.io>
|
||||
*
|
||||
* This work is licensed under the MIT license, see the file LICENSE for details.
|
||||
*
|
||||
* VOSPI driver.
|
||||
*/
|
||||
|
||||
#ifndef __VOSPI_H__
|
||||
#define __VOSPI_H__
|
||||
int vospi_init(uint32_t n_packets, void *buffer);
|
||||
int vospi_snapshot(uint32_t timeout_ms);
|
||||
#endif // __VOSPI_H__
|
||||
@ -150,6 +150,7 @@ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/common/, \
|
||||
usbdbg.o \
|
||||
sensor_utils.o \
|
||||
factoryreset.o \
|
||||
vospi.o \
|
||||
)
|
||||
|
||||
FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/sensors/, \
|
||||
@ -532,6 +533,7 @@ UVC_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/common/, \
|
||||
trace.o \
|
||||
mutex.o \
|
||||
sensor_utils.o \
|
||||
vospi.o \
|
||||
)
|
||||
|
||||
UVC_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/sensors/, \
|
||||
|
||||
@ -1,8 +1,8 @@
|
||||
/*
|
||||
* This file is part of the OpenMV project.
|
||||
*
|
||||
* Copyright (c) 2013-2021 Ibrahim Abdelkader <iabdalkader@openmv.io>
|
||||
* Copyright (c) 2013-2021 Kwabena W. Agyeman <kwagyeman@openmv.io>
|
||||
* Copyright (c) 2013-2023 Ibrahim Abdelkader <iabdalkader@openmv.io>
|
||||
* Copyright (c) 2013-2023 Kwabena W. Agyeman <kwagyeman@openmv.io>
|
||||
*
|
||||
* This work is licensed under the MIT license, see the file LICENSE for details.
|
||||
*
|
||||
@ -10,20 +10,16 @@
|
||||
*/
|
||||
#include "omv_boardconfig.h"
|
||||
#if (OMV_ENABLE_LEPTON == 1)
|
||||
#include STM32_HAL_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include "py/mphal.h"
|
||||
#include "irq.h"
|
||||
|
||||
#include "framebuffer.h"
|
||||
#include "common.h"
|
||||
#include "dma_alloc.h"
|
||||
#include "omv_i2c.h"
|
||||
#include "omv_gpio.h"
|
||||
#include "dma_utils.h"
|
||||
#include "sensor.h"
|
||||
#include "vospi.h"
|
||||
#include "py/mphal.h"
|
||||
#include "common.h"
|
||||
#include "omv_gpio.h"
|
||||
#include "omv_i2c.h"
|
||||
#include "framebuffer.h"
|
||||
|
||||
#include "crc16.h"
|
||||
#include "LEPTON_SDK.h"
|
||||
#include "LEPTON_AGC.h"
|
||||
#include "LEPTON_SYS.h"
|
||||
@ -32,76 +28,37 @@
|
||||
#include "LEPTON_RAD.h"
|
||||
#include "LEPTON_I2C_Reg.h"
|
||||
|
||||
#define VOSPI_LINE_PIXELS (80)
|
||||
#define VOSPI_NUMBER_PACKETS (60)
|
||||
#define VOSPI_SPECIAL_PACKET (20)
|
||||
#define VOSPI_LINE_SIZE (80 * 2)
|
||||
#define VOSPI_HEADER_SIZE (4)
|
||||
#define VOSPI_PACKET_SIZE (VOSPI_HEADER_SIZE + VOSPI_LINE_SIZE)
|
||||
#define VOSPI_HEADER_SEG(buf) (((buf[0] >> 4) & 0x7))
|
||||
#define VOSPI_HEADER_PID(buf) (((buf[0] << 8) | (buf[1] << 0)) & 0x0FFF)
|
||||
#define VOSPI_HEADER_CRC(buf) (((buf[2] << 8) | (buf[3] << 0)))
|
||||
#define VOSPI_FIRST_PACKET (0)
|
||||
#define VOSPI_FIRST_SEGMENT (1)
|
||||
#define LEPTON_TIMEOUT (1000)
|
||||
// Temperatures in Celsius
|
||||
#define DEFAULT_MIN_TEMP (-10.0f)
|
||||
#define DEFAULT_MAX_TEMP (40.0f)
|
||||
#define LEPTON_BOOT_TIMEOUT (1000)
|
||||
#define LEPTON_SNAPSHOT_RETRY (3)
|
||||
#define LEPTON_SNAPSHOT_TIMEOUT (10000)
|
||||
|
||||
// Min/Max temperatures in Celsius.
|
||||
#define LEPTON_MIN_TEMP_NORM (-10.0f)
|
||||
#define LEPTON_MAX_TEMP_NORM (140.0f)
|
||||
#define LEPTON_MIN_TEMP_HIGH (-10.0f)
|
||||
#define LEPTON_MIN_TEMP_DEFAULT (-10.0f)
|
||||
|
||||
#define LEPTON_MAX_TEMP_NORM (140.0f)
|
||||
#define LEPTON_MAX_TEMP_HIGH (600.0f)
|
||||
#define LEPTON_MAX_TEMP_DEFAULT (40.0f)
|
||||
|
||||
static bool radiometry = false;
|
||||
static int h_res = 0;
|
||||
static int v_res = 0;
|
||||
static bool v_flip = false;
|
||||
static bool h_mirror = false;
|
||||
static bool measurement_mode = false;
|
||||
static bool high_temp_mode = false;
|
||||
static float min_temp = DEFAULT_MIN_TEMP;
|
||||
static float max_temp = DEFAULT_MAX_TEMP;
|
||||
typedef struct lepton_state {
|
||||
int h_res;
|
||||
int v_res;
|
||||
bool vflip;
|
||||
bool hmirror;
|
||||
float min_temp;
|
||||
float max_temp;
|
||||
bool radiometry;
|
||||
bool high_temp_mode;
|
||||
bool measurement_mode;
|
||||
LEP_CAMERA_PORT_DESC_T port;
|
||||
} lepton_state_t;
|
||||
|
||||
extern SPI_HandleTypeDef ISC_SPIHandle;
|
||||
static DMA_HandleTypeDef DMAHandle;
|
||||
LEP_CAMERA_PORT_DESC_T LEPHandle;
|
||||
extern uint8_t _vospi_buf[];
|
||||
extern uint16_t _vospi_buf[];
|
||||
static lepton_state_t lepton;
|
||||
|
||||
static bool vospi_resync = true;
|
||||
static uint8_t *vospi_packet = NULL;
|
||||
static uint8_t *vospi_buffer = _vospi_buf;
|
||||
static volatile uint32_t vospi_pid = 0;
|
||||
static volatile uint32_t vospi_seg = 1;
|
||||
static uint32_t vospi_packets = 60;
|
||||
static int lepton_reset(sensor_t *sensor, bool measurement_mode, bool high_temp_mode);
|
||||
|
||||
static void lepton_sync()
|
||||
{
|
||||
HAL_SPI_Abort(&ISC_SPIHandle);
|
||||
|
||||
// Disable DMA IRQ
|
||||
HAL_NVIC_DisableIRQ(ISC_SPI_DMA_IRQn);
|
||||
|
||||
debug_printf("resync...\n");
|
||||
mp_hal_delay_ms(200);
|
||||
|
||||
vospi_resync = false;
|
||||
vospi_pid = VOSPI_FIRST_PACKET;
|
||||
vospi_seg = VOSPI_FIRST_SEGMENT;
|
||||
|
||||
HAL_NVIC_EnableIRQ(ISC_SPI_DMA_IRQn);
|
||||
HAL_SPI_Receive_DMA(&ISC_SPIHandle, vospi_packet, VOSPI_PACKET_SIZE);
|
||||
}
|
||||
|
||||
static uint16_t lepton_calc_crc(uint8_t *buf)
|
||||
{
|
||||
buf[0] &= 0x0F;
|
||||
buf[1] &= 0xFF;
|
||||
buf[2] = 0;
|
||||
buf[3] = 0;
|
||||
return CalcCRC16Bytes(VOSPI_PACKET_SIZE, (char *) buf);
|
||||
}
|
||||
|
||||
static int sleep(sensor_t *sensor, int enable)
|
||||
{
|
||||
if (enable) {
|
||||
@ -206,13 +163,13 @@ static int get_rgb_gain_db(sensor_t *sensor, float *r_gain_db, float *g_gain_db,
|
||||
|
||||
static int set_hmirror(sensor_t *sensor, int enable)
|
||||
{
|
||||
h_mirror = enable;
|
||||
lepton.hmirror = enable;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int set_vflip(sensor_t *sensor, int enable)
|
||||
{
|
||||
v_flip = enable;
|
||||
lepton.vflip = enable;
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -225,29 +182,29 @@ static int ioctl(sensor_t *sensor, int request, va_list ap)
|
||||
{
|
||||
int ret = 0;
|
||||
|
||||
if ((!h_res) || (!v_res)) {
|
||||
if ((!lepton.h_res) || (!lepton.v_res)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
switch (request) {
|
||||
case IOCTL_LEPTON_GET_WIDTH: {
|
||||
int *width = va_arg(ap, int *);
|
||||
*width = h_res;
|
||||
*width = lepton.h_res;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_HEIGHT: {
|
||||
int *height = va_arg(ap, int *);
|
||||
*height = v_res;
|
||||
*height = lepton.v_res;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_RADIOMETRY: {
|
||||
int *type = va_arg(ap, int *);
|
||||
*type = radiometry;
|
||||
*type = lepton.radiometry;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_REFRESH: {
|
||||
int *refresh = va_arg(ap, int *);
|
||||
*refresh = (h_res == 80) ? 27 : 9;
|
||||
*refresh = (lepton.h_res == 80) ? 27 : 9;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_RESOLUTION: {
|
||||
@ -257,68 +214,68 @@ static int ioctl(sensor_t *sensor, int request, va_list ap)
|
||||
}
|
||||
case IOCTL_LEPTON_RUN_COMMAND: {
|
||||
int command = va_arg(ap, int);
|
||||
ret = (LEP_RunCommand(&LEPHandle, command) == LEP_OK) ? 0 : -1;
|
||||
ret = (LEP_RunCommand(&lepton.port, command) == LEP_OK) ? 0 : -1;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_SET_ATTRIBUTE: {
|
||||
int command = va_arg(ap, int);
|
||||
uint16_t *data = va_arg(ap, uint16_t *);
|
||||
size_t data_len = va_arg(ap, size_t);
|
||||
ret = (LEP_SetAttribute(&LEPHandle, command, (LEP_ATTRIBUTE_T_PTR) data, data_len) == LEP_OK) ? 0 : -1;
|
||||
ret = (LEP_SetAttribute(&lepton.port, command, (LEP_ATTRIBUTE_T_PTR) data, data_len) == LEP_OK) ? 0 : -1;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_ATTRIBUTE: {
|
||||
int command = va_arg(ap, int);
|
||||
uint16_t *data = va_arg(ap, uint16_t *);
|
||||
size_t data_len = va_arg(ap, size_t);
|
||||
ret = (LEP_GetAttribute(&LEPHandle, command, (LEP_ATTRIBUTE_T_PTR) data, data_len) == LEP_OK) ? 0 : -1;
|
||||
ret = (LEP_GetAttribute(&lepton.port, command, (LEP_ATTRIBUTE_T_PTR) data, data_len) == LEP_OK) ? 0 : -1;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_FPA_TEMPERATURE: {
|
||||
int *temp = va_arg(ap, int *);
|
||||
LEP_SYS_FPA_TEMPERATURE_KELVIN_T tfpa;
|
||||
ret = (LEP_GetSysFpaTemperatureKelvin(&LEPHandle, &tfpa) == LEP_OK) ? 0 : -1;
|
||||
ret = (LEP_GetSysFpaTemperatureKelvin(&lepton.port, &tfpa) == LEP_OK) ? 0 : -1;
|
||||
*temp = tfpa;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_AUX_TEMPERATURE: {
|
||||
int *temp = va_arg(ap, int *);
|
||||
LEP_SYS_AUX_TEMPERATURE_KELVIN_T taux;
|
||||
ret = (LEP_GetSysAuxTemperatureKelvin(&LEPHandle, &taux) == LEP_OK) ? 0 : -1;
|
||||
ret = (LEP_GetSysAuxTemperatureKelvin(&lepton.port, &taux) == LEP_OK) ? 0 : -1;
|
||||
*temp = taux;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_SET_MEASUREMENT_MODE: {
|
||||
int measurement_mode_in = va_arg(ap, int);
|
||||
int high_temp_mode_in = va_arg(ap, int);
|
||||
if (measurement_mode != measurement_mode_in) {
|
||||
measurement_mode = measurement_mode_in;
|
||||
high_temp_mode = high_temp_mode_in;
|
||||
ret = lepton_reset(sensor, measurement_mode, high_temp_mode);
|
||||
if (lepton.measurement_mode != measurement_mode_in) {
|
||||
lepton.measurement_mode = measurement_mode_in;
|
||||
lepton.high_temp_mode = high_temp_mode_in;
|
||||
ret = lepton_reset(sensor, lepton.measurement_mode, lepton.high_temp_mode);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_MEASUREMENT_MODE: {
|
||||
int *measurement_mode_out = va_arg(ap, int *);
|
||||
int *high_temp_mode_out = va_arg(ap, int *);
|
||||
*measurement_mode_out = measurement_mode;
|
||||
*high_temp_mode_out = high_temp_mode;
|
||||
*measurement_mode_out = lepton.measurement_mode;
|
||||
*high_temp_mode_out = lepton.high_temp_mode;
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_SET_MEASUREMENT_RANGE: {
|
||||
float *arg_min_temp = va_arg(ap, float *);
|
||||
float *arg_max_temp = va_arg(ap, float *);
|
||||
float min_temp_range = (high_temp_mode) ? LEPTON_MIN_TEMP_HIGH : LEPTON_MIN_TEMP_NORM;
|
||||
float max_temp_range = (high_temp_mode) ? LEPTON_MAX_TEMP_HIGH : LEPTON_MAX_TEMP_NORM;
|
||||
min_temp = IM_MAX(IM_MIN(*arg_min_temp, *arg_max_temp), min_temp_range);
|
||||
max_temp = IM_MIN(IM_MAX(*arg_max_temp, *arg_min_temp), max_temp_range);
|
||||
float min_temp_range = (lepton.high_temp_mode) ? LEPTON_MIN_TEMP_HIGH : LEPTON_MIN_TEMP_NORM;
|
||||
float max_temp_range = (lepton.high_temp_mode) ? LEPTON_MAX_TEMP_HIGH : LEPTON_MAX_TEMP_NORM;
|
||||
lepton.min_temp = IM_MAX(IM_MIN(*arg_min_temp, *arg_max_temp), min_temp_range);
|
||||
lepton.max_temp = IM_MIN(IM_MAX(*arg_max_temp, *arg_min_temp), max_temp_range);
|
||||
break;
|
||||
}
|
||||
case IOCTL_LEPTON_GET_MEASUREMENT_RANGE: {
|
||||
float *ptr_min_temp = va_arg(ap, float *);
|
||||
float *ptr_max_temp = va_arg(ap, float *);
|
||||
*ptr_min_temp = min_temp;
|
||||
*ptr_max_temp = max_temp;
|
||||
*ptr_min_temp = lepton.min_temp;
|
||||
*ptr_max_temp = lepton.max_temp;
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
@ -346,128 +303,88 @@ static int lepton_reset(sensor_t *sensor, bool measurement_mode, bool high_temp_
|
||||
|
||||
LEP_RAD_ENABLE_E rad;
|
||||
LEP_AGC_ROI_T roi;
|
||||
memset(&LEPHandle, 0, sizeof(LEP_CAMERA_PORT_DESC_T));
|
||||
memset(&lepton.port, 0, sizeof(LEP_CAMERA_PORT_DESC_T));
|
||||
|
||||
for (mp_uint_t start = mp_hal_ticks_ms(); ;mp_hal_delay_ms(1)) {
|
||||
if (LEP_OpenPort(&sensor->i2c_bus, LEP_CCI_TWI, 0, &LEPHandle) == LEP_OK) {
|
||||
if (LEP_OpenPort(&sensor->i2c_bus, LEP_CCI_TWI, 0, &lepton.port) == LEP_OK) {
|
||||
break;
|
||||
}
|
||||
if ((mp_hal_ticks_ms() - start) >= LEPTON_TIMEOUT) {
|
||||
if ((mp_hal_ticks_ms() - start) >= LEPTON_BOOT_TIMEOUT) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
for (mp_uint_t start = mp_hal_ticks_ms(); ;mp_hal_delay_ms(1)) {
|
||||
LEP_SDK_BOOT_STATUS_E status;
|
||||
if (LEP_GetCameraBootStatus(&LEPHandle, &status) != LEP_OK) {
|
||||
if (LEP_GetCameraBootStatus(&lepton.port, &status) != LEP_OK) {
|
||||
return -1;
|
||||
}
|
||||
if (status == LEP_BOOT_STATUS_BOOTED) {
|
||||
break;
|
||||
}
|
||||
if ((mp_hal_ticks_ms() - start) >= LEPTON_TIMEOUT) {
|
||||
if ((mp_hal_ticks_ms() - start) >= LEPTON_BOOT_TIMEOUT) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
for (mp_uint_t start = mp_hal_ticks_ms(); ;mp_hal_delay_ms(1)) {
|
||||
LEP_UINT16 status;
|
||||
if (LEP_DirectReadRegister(&LEPHandle, LEP_I2C_STATUS_REG, &status) != LEP_OK) {
|
||||
if (LEP_DirectReadRegister(&lepton.port, LEP_I2C_STATUS_REG, &status) != LEP_OK) {
|
||||
return -1;
|
||||
}
|
||||
if (!(status & LEP_I2C_STATUS_BUSY_BIT_MASK)) {
|
||||
break;
|
||||
}
|
||||
if ((mp_hal_ticks_ms() - start) >= LEPTON_TIMEOUT) {
|
||||
if ((mp_hal_ticks_ms() - start) >= LEPTON_BOOT_TIMEOUT) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
if (LEP_GetRadEnableState(&LEPHandle, &rad) != LEP_OK
|
||||
|| LEP_GetAgcROI(&LEPHandle, &roi) != LEP_OK) {
|
||||
if (LEP_GetRadEnableState(&lepton.port, &rad) != LEP_OK
|
||||
|| LEP_GetAgcROI(&lepton.port, &roi) != LEP_OK) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Use the low gain mode to enable high temperature readings (~450C) on Lepton 3.5
|
||||
LEP_SYS_GAIN_MODE_E gain_mode = high_temp_mode ? LEP_SYS_GAIN_MODE_LOW : LEP_SYS_GAIN_MODE_HIGH;
|
||||
if (LEP_SetSysGainMode(&LEPHandle, gain_mode) != LEP_OK) {
|
||||
LEP_SYS_GAIN_MODE_E gain_mode = lepton.high_temp_mode ? LEP_SYS_GAIN_MODE_LOW : LEP_SYS_GAIN_MODE_HIGH;
|
||||
if (LEP_SetSysGainMode(&lepton.port, gain_mode) != LEP_OK) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!measurement_mode) {
|
||||
if (LEP_SetRadEnableState(&LEPHandle, LEP_RAD_DISABLE) != LEP_OK
|
||||
|| LEP_SetAgcEnableState(&LEPHandle, LEP_AGC_ENABLE) != LEP_OK
|
||||
|| LEP_SetAgcCalcEnableState(&LEPHandle, LEP_AGC_ENABLE) != LEP_OK) {
|
||||
if (!lepton.measurement_mode) {
|
||||
if (LEP_SetRadEnableState(&lepton.port, LEP_RAD_DISABLE) != LEP_OK
|
||||
|| LEP_SetAgcEnableState(&lepton.port, LEP_AGC_ENABLE) != LEP_OK
|
||||
|| LEP_SetAgcCalcEnableState(&lepton.port, LEP_AGC_ENABLE) != LEP_OK) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
h_res = roi.endCol + 1;
|
||||
v_res = roi.endRow + 1;
|
||||
radiometry = (rad == LEP_RAD_ENABLE);
|
||||
|
||||
if (v_res > 60) {
|
||||
vospi_packets = 240;
|
||||
} else {
|
||||
vospi_packets = 60;
|
||||
}
|
||||
|
||||
// resync and enable DMA before the first snapshot.
|
||||
vospi_resync = true;
|
||||
lepton.h_res = roi.endCol + 1;
|
||||
lepton.v_res = roi.endRow + 1;
|
||||
lepton.radiometry = (rad == LEP_RAD_ENABLE);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int reset(sensor_t *sensor)
|
||||
{
|
||||
h_res = 0;
|
||||
v_res = 0;
|
||||
v_flip = false;
|
||||
h_mirror = false;
|
||||
radiometry = false;
|
||||
measurement_mode = false;
|
||||
high_temp_mode = false;
|
||||
min_temp = DEFAULT_MIN_TEMP;
|
||||
max_temp = DEFAULT_MAX_TEMP;
|
||||
return lepton_reset(sensor, false, false);
|
||||
}
|
||||
static bool vospi_initialized = false;
|
||||
|
||||
void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi)
|
||||
{
|
||||
(void) lepton_calc_crc; // to shut the compiler up.
|
||||
memset(&lepton, 0, sizeof(lepton_state_t));
|
||||
lepton.min_temp = LEPTON_MIN_TEMP_DEFAULT;
|
||||
lepton.max_temp = LEPTON_MAX_TEMP_DEFAULT;
|
||||
|
||||
if (vospi_resync == true) {
|
||||
return; // nothing to do here
|
||||
if (lepton_reset(sensor, false, false) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (vospi_pid < vospi_packets && (vospi_packet[0] & 0xF) != 0xF) {
|
||||
uint32_t pid = VOSPI_HEADER_PID(vospi_packet);
|
||||
uint32_t seg = VOSPI_HEADER_SEG(vospi_packet);
|
||||
if (pid != (vospi_pid % VOSPI_NUMBER_PACKETS)) {
|
||||
if (vospi_pid == VOSPI_FIRST_PACKET) {
|
||||
// Wait for the first packet of the first segement.
|
||||
vospi_pid = VOSPI_FIRST_PACKET;
|
||||
vospi_seg = VOSPI_FIRST_SEGMENT;
|
||||
} else { // lost sync
|
||||
vospi_resync = true;
|
||||
debug_printf("lost sync, packet id:%lu expected id:%lu \n", pid, vospi_pid);
|
||||
}
|
||||
} else if (vospi_packets > 60 && pid == VOSPI_SPECIAL_PACKET && seg != vospi_seg ) {
|
||||
if (vospi_seg == VOSPI_FIRST_SEGMENT) {
|
||||
// Wait for the first packet of the first segement.
|
||||
vospi_pid = VOSPI_FIRST_PACKET;
|
||||
vospi_seg = VOSPI_FIRST_SEGMENT;
|
||||
} else { // lost sync
|
||||
vospi_resync = true;
|
||||
debug_printf("lost sync, segment id:%lu expected id:%lu\n", seg, vospi_seg);
|
||||
}
|
||||
} else {
|
||||
memcpy(vospi_buffer + vospi_pid * VOSPI_LINE_SIZE,
|
||||
vospi_packet + VOSPI_HEADER_SIZE, VOSPI_LINE_SIZE);
|
||||
if ((++vospi_pid % VOSPI_NUMBER_PACKETS) == 0) {
|
||||
vospi_seg++;
|
||||
}
|
||||
if (vospi_initialized == false) {
|
||||
if (vospi_init(lepton.v_res, _vospi_buf) != 0) {
|
||||
return -1;
|
||||
}
|
||||
vospi_initialized = true;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int snapshot(sensor_t *sensor, image_t *image, uint32_t flags)
|
||||
@ -482,7 +399,7 @@ static int snapshot(sensor_t *sensor, image_t *image, uint32_t flags)
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((!h_res) || (!v_res) || (!sensor->framesize) || (!sensor->pixformat)) {
|
||||
if ((!lepton.h_res) || (!lepton.v_res) || (!sensor->framesize) || (!sensor->pixformat)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -493,53 +410,18 @@ static int snapshot(sensor_t *sensor, image_t *image, uint32_t flags)
|
||||
return -1;
|
||||
}
|
||||
|
||||
// The SPI DMA device is always clocking the FLIR Lepton in the background.
|
||||
// The code below resets the vospi control values to let data be pulled in.
|
||||
// If we need to re-sync we do it. Otherwise, after we finish pulling data
|
||||
// in we exit and let the SPI bus keep running. Then on the next call to
|
||||
// snapshot we read in more data and pull in the next frame.
|
||||
HAL_NVIC_DisableIRQ(ISC_SPI_DMA_IRQn);
|
||||
vospi_pid = VOSPI_FIRST_PACKET;
|
||||
vospi_seg = VOSPI_FIRST_SEGMENT;
|
||||
HAL_NVIC_EnableIRQ(ISC_SPI_DMA_IRQn);
|
||||
|
||||
// Snapshot start tick
|
||||
mp_uint_t tick_start = mp_hal_ticks_ms();
|
||||
bool reset_tried = false;
|
||||
|
||||
do {
|
||||
if (vospi_resync == true) {
|
||||
lepton_sync();
|
||||
for (int i=0; i<LEPTON_SNAPSHOT_RETRY; i++) {
|
||||
if (vospi_snapshot(LEPTON_SNAPSHOT_TIMEOUT) == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
__WFI();
|
||||
|
||||
if ((mp_hal_ticks_ms() - tick_start) >= 20000) {
|
||||
// Timeout error.
|
||||
if (i+1 == LEPTON_SNAPSHOT_RETRY) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((!reset_tried) && ((mp_hal_ticks_ms() - tick_start) >= 10000)) {
|
||||
reset_tried = true;
|
||||
|
||||
// The FLIR lepton might have crashed so reset it (it does this).
|
||||
bool temp_h_mirror = h_mirror;
|
||||
bool temp_v_flip = v_flip;
|
||||
int ret = lepton_reset(sensor, measurement_mode, high_temp_mode);
|
||||
h_mirror = temp_h_mirror;
|
||||
v_flip = temp_v_flip;
|
||||
|
||||
if (ret < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Reset the VOSPI interface again.
|
||||
HAL_NVIC_DisableIRQ(ISC_SPI_DMA_IRQn);
|
||||
vospi_pid = VOSPI_FIRST_PACKET;
|
||||
vospi_seg = VOSPI_FIRST_SEGMENT;
|
||||
HAL_NVIC_EnableIRQ(ISC_SPI_DMA_IRQn);
|
||||
// The FLIR lepton might have crashed so reset it (it does this).
|
||||
if (lepton_reset(sensor, lepton.measurement_mode, lepton.high_temp_mode) != 0) {
|
||||
return -1;
|
||||
}
|
||||
} while (vospi_pid < vospi_packets); // only checking one volatile var so atomic.
|
||||
}
|
||||
|
||||
MAIN_FB()->w = MAIN_FB()->u;
|
||||
MAIN_FB()->h = MAIN_FB()->v;
|
||||
@ -547,49 +429,54 @@ static int snapshot(sensor_t *sensor, image_t *image, uint32_t flags)
|
||||
|
||||
framebuffer_init_image(image);
|
||||
|
||||
uint16_t *src = (uint16_t*) vospi_buffer;
|
||||
|
||||
float x_scale = resolution[sensor->framesize][0] / ((float) h_res);
|
||||
float y_scale = resolution[sensor->framesize][1] / ((float) v_res);
|
||||
float x_scale = resolution[sensor->framesize][0] / ((float) lepton.h_res);
|
||||
float y_scale = resolution[sensor->framesize][1] / ((float) lepton.v_res);
|
||||
// MAX == KeepAspectRationByExpanding - MIN == KeepAspectRatio
|
||||
float scale = IM_MAX(x_scale, y_scale), scale_inv = 1.0f / scale;
|
||||
int x_offset = (resolution[sensor->framesize][0] - (h_res * scale)) / 2;
|
||||
int y_offset = (resolution[sensor->framesize][1] - (v_res * scale)) / 2;
|
||||
int x_offset = (resolution[sensor->framesize][0] - (lepton.h_res * scale)) / 2;
|
||||
int y_offset = (resolution[sensor->framesize][1] - (lepton.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.
|
||||
|
||||
LEP_SYS_FPA_TEMPERATURE_KELVIN_T kelvin;
|
||||
if (measurement_mode && (!radiometry)) {
|
||||
if (LEP_GetSysFpaTemperatureKelvin(&LEPHandle, &kelvin) != LEP_OK) {
|
||||
if (lepton.measurement_mode && (!lepton.radiometry)) {
|
||||
if (LEP_GetSysFpaTemperatureKelvin(&lepton.port, &kelvin) != LEP_OK) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
for (int y = y_offset, yy = fast_ceilf(v_res * scale) + y_offset; y < yy; y++) {
|
||||
for (int y = y_offset, yy = fast_ceilf(lepton.v_res * scale) + y_offset; y < yy; y++) {
|
||||
if ((MAIN_FB()->y <= y) && (y < (MAIN_FB()->y + MAIN_FB()->v))) { // user window cropping
|
||||
|
||||
uint16_t *row_ptr = src + (fast_floorf(y * scale_inv) * h_res);
|
||||
uint16_t *row_ptr = _vospi_buf + (fast_floorf(y * scale_inv) * lepton.h_res);
|
||||
|
||||
for (int x = x_offset, xx = fast_ceilf(h_res * scale) + x_offset; x < xx; x++) {
|
||||
for (int x = x_offset, xx = fast_ceilf(lepton.h_res * scale) + x_offset; x < xx; x++) {
|
||||
if ((MAIN_FB()->x <= x) && (x < (MAIN_FB()->x + MAIN_FB()->u))) { // user window cropping
|
||||
|
||||
// Value is the 14/16-bit value from the FLIR IR camera.
|
||||
// However, with AGC enabled only the bottom 8-bits are non-zero.
|
||||
int value = __REV16(row_ptr[fast_floorf(x * scale_inv)]);
|
||||
|
||||
if (measurement_mode) {
|
||||
if (lepton.measurement_mode) {
|
||||
// Need to convert 14/16-bits to 8-bits ourselves...
|
||||
if (!radiometry) value = (value - 8192) + kelvin;
|
||||
if (!lepton.radiometry) {
|
||||
value = (value - 8192) + kelvin;
|
||||
}
|
||||
float celsius = (value * 0.01f) - 273.15f;
|
||||
celsius = IM_MAX(IM_MIN(celsius, max_temp), min_temp);
|
||||
value = IM_MAX(IM_MIN(IM_DIV(((celsius - min_temp) * 255), (max_temp - min_temp)), 255), 0);
|
||||
celsius = IM_MAX(IM_MIN(celsius, lepton.max_temp), lepton.min_temp);
|
||||
value = IM_MAX(IM_MIN(IM_DIV(((celsius - lepton.min_temp) * 255),
|
||||
(lepton.max_temp - lepton.min_temp)), 255), 0);
|
||||
}
|
||||
|
||||
int t_x = x - MAIN_FB()->x;
|
||||
int t_y = y - MAIN_FB()->y;
|
||||
|
||||
if (h_mirror) t_x = MAIN_FB()->u - t_x - 1;
|
||||
if (v_flip) t_y = MAIN_FB()->v - t_y - 1;
|
||||
if (lepton.hmirror) {
|
||||
t_x = MAIN_FB()->u - t_x - 1;
|
||||
}
|
||||
if (lepton.vflip) {
|
||||
t_y = MAIN_FB()->v - t_y - 1;
|
||||
}
|
||||
|
||||
switch (sensor->pixformat) {
|
||||
case PIXFORMAT_GRAYSCALE: {
|
||||
@ -646,100 +533,31 @@ int lepton_init(sensor_t *sensor)
|
||||
sensor->hw_flags.jpege = 0;
|
||||
sensor->hw_flags.gs_bpp = 1;
|
||||
|
||||
// Allocate packet buffer in the same domain as the DMA instance.
|
||||
vospi_packet = dma_alloc(VOSPI_PACKET_SIZE, ISC_SPI_DMA_STREAM);
|
||||
if (vospi_packet == NULL) {
|
||||
if (reset(sensor) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Configure the DMA handler for Transmission process
|
||||
DMAHandle.Instance = ISC_SPI_DMA_STREAM;
|
||||
DMAHandle.Init.Request = ISC_SPI_DMA_REQUEST;
|
||||
DMAHandle.Init.Mode = DMA_CIRCULAR;
|
||||
DMAHandle.Init.Priority = DMA_PRIORITY_HIGH;
|
||||
DMAHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
|
||||
// When the DMA is configured in direct mode (the FIFO is disabled), the source and
|
||||
// destination transfer widths are equal, and both defined by PSIZE (MSIZE is ignored).
|
||||
// Additionally, burst transfers are not possible (MBURST and PBURST are both ignored).
|
||||
DMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
|
||||
DMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
|
||||
// Note MBURST and PBURST are ignored.
|
||||
DMAHandle.Init.MemBurst = DMA_MBURST_INC4;
|
||||
DMAHandle.Init.PeriphBurst = DMA_PBURST_INC4;
|
||||
DMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
|
||||
DMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
|
||||
DMAHandle.Init.MemInc = DMA_MINC_ENABLE;
|
||||
DMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
|
||||
|
||||
// NVIC configuration for DMA transfer complete interrupt
|
||||
NVIC_SetPriority(ISC_SPI_DMA_IRQn, IRQ_PRI_DMA21);
|
||||
HAL_NVIC_DisableIRQ(ISC_SPI_DMA_IRQn);
|
||||
|
||||
#if defined(ISC_SPI_DMA_CLK_ENABLE)
|
||||
ISC_SPI_DMA_CLK_ENABLE();
|
||||
#endif
|
||||
|
||||
HAL_DMA_DeInit(&DMAHandle);
|
||||
if (HAL_DMA_Init(&DMAHandle) != HAL_OK) {
|
||||
// Initialization Error
|
||||
return -1;
|
||||
}
|
||||
|
||||
memset(&ISC_SPIHandle, 0, sizeof(ISC_SPIHandle));
|
||||
ISC_SPIHandle.Instance = ISC_SPI;
|
||||
ISC_SPIHandle.Init.NSS = SPI_NSS_HARD_OUTPUT;
|
||||
ISC_SPIHandle.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
|
||||
ISC_SPIHandle.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
|
||||
ISC_SPIHandle.Init.Mode = SPI_MODE_MASTER;
|
||||
ISC_SPIHandle.Init.TIMode = SPI_TIMODE_DISABLE;
|
||||
ISC_SPIHandle.Init.Direction = SPI_DIRECTION_2LINES_RXONLY;
|
||||
ISC_SPIHandle.Init.DataSize = SPI_DATASIZE_8BIT;
|
||||
ISC_SPIHandle.Init.FifoThreshold = SPI_FIFO_THRESHOLD_04DATA;
|
||||
ISC_SPIHandle.Init.FirstBit = SPI_FIRSTBIT_MSB;
|
||||
ISC_SPIHandle.Init.CLKPhase = SPI_PHASE_2EDGE;
|
||||
ISC_SPIHandle.Init.CLKPolarity = SPI_POLARITY_HIGH;
|
||||
ISC_SPIHandle.Init.BaudRatePrescaler = ISC_SPI_PRESCALER;
|
||||
// Recommanded setting to avoid glitches
|
||||
ISC_SPIHandle.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE;
|
||||
|
||||
if (HAL_SPI_Init(&ISC_SPIHandle) != HAL_OK) {
|
||||
ISC_SPI_RESET();
|
||||
ISC_SPI_RELEASE();
|
||||
ISC_SPI_CLK_DISABLE();
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Associate the initialized DMA handle to the the SPI handle
|
||||
__HAL_LINKDMA(&ISC_SPIHandle, hdmarx, DMAHandle);
|
||||
|
||||
// Set DMA IRQ handle
|
||||
dma_utils_set_irq_descr(ISC_SPI_DMA_STREAM, &DMAHandle);
|
||||
|
||||
// NVIC configuration for SPI transfer complete interrupt
|
||||
NVIC_SetPriority(ISC_SPI_IRQn, IRQ_PRI_DCMI);
|
||||
HAL_NVIC_EnableIRQ(ISC_SPI_IRQn);
|
||||
|
||||
LEP_OEM_PART_NUMBER_T part;
|
||||
if ((!reset(sensor))
|
||||
&& (LEP_GetOemFlirPartNumber(&LEPHandle, &part) == LEP_OK)) {
|
||||
// 500 == Lepton
|
||||
// xxxx == Version
|
||||
// 01/00 == Shutter/NoShutter
|
||||
if (!strncmp(part.value, "500-0771", 8)) {
|
||||
sensor->chip_id_w = LEPTON_3_5;
|
||||
} else if (!strncmp(part.value, "500-0726", 8)) {
|
||||
sensor->chip_id_w = LEPTON_3_0;
|
||||
} else if (!strncmp(part.value, "500-0763", 8)) {
|
||||
sensor->chip_id_w = LEPTON_2_5;
|
||||
} else if (!strncmp(part.value, "500-0659", 8)) {
|
||||
sensor->chip_id_w = LEPTON_2_0;
|
||||
} else if (!strncmp(part.value, "500-0690", 8)) {
|
||||
sensor->chip_id_w = LEPTON_1_6;
|
||||
} else if (!strncmp(part.value, "500-0643", 8)) {
|
||||
sensor->chip_id_w = LEPTON_1_5;
|
||||
}
|
||||
if (LEP_GetOemFlirPartNumber(&lepton.port, &part) != LEP_OK) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 500 == Lepton
|
||||
// xxxx == Version
|
||||
// 01/00 == Shutter/NoShutter
|
||||
if (!strncmp(part.value, "500-0771", 8)) {
|
||||
sensor->chip_id_w = LEPTON_3_5;
|
||||
} else if (!strncmp(part.value, "500-0726", 8)) {
|
||||
sensor->chip_id_w = LEPTON_3_0;
|
||||
} else if (!strncmp(part.value, "500-0763", 8)) {
|
||||
sensor->chip_id_w = LEPTON_2_5;
|
||||
} else if (!strncmp(part.value, "500-0659", 8)) {
|
||||
sensor->chip_id_w = LEPTON_2_0;
|
||||
} else if (!strncmp(part.value, "500-0690", 8)) {
|
||||
sensor->chip_id_w = LEPTON_1_6;
|
||||
} else if (!strncmp(part.value, "500-0643", 8)) {
|
||||
sensor->chip_id_w = LEPTON_1_5;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
#endif // (OMV_ENABLE_LEPTON == 1)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user