mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
Add ToF module.
This commit is contained in:
parent
f7aa217f39
commit
0363fa8758
@ -179,6 +179,7 @@ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/py/, \
|
||||
py_gif.o \
|
||||
py_mjpeg.o \
|
||||
py_winc.o \
|
||||
py_tof.o \
|
||||
)
|
||||
|
||||
ifeq ($(TARGET), OPENMV1)
|
||||
|
||||
@ -1 +1 @@
|
||||
Subproject commit d3b0adda9a99d3e9abe92de913955ab3db2f8d5b
|
||||
Subproject commit 9038f0e4c9a090ca4b845eba30e2caa126f31b9a
|
||||
@ -64,6 +64,7 @@ SRCS += $(addprefix py/, \
|
||||
py_gif.c \
|
||||
py_mjpeg.c \
|
||||
py_winc.c \
|
||||
py_tof.c \
|
||||
)
|
||||
|
||||
OBJS = $(addprefix $(BUILD)/, $(SRCS:.c=.o))
|
||||
|
||||
@ -66,6 +66,7 @@
|
||||
#include "py_image.h"
|
||||
#include "py_lcd.h"
|
||||
#include "py_fir.h"
|
||||
#include "py_tof.h"
|
||||
|
||||
int errno;
|
||||
extern char _vfs_buf;
|
||||
@ -338,6 +339,7 @@ soft_reset:
|
||||
file_buffer_init0();
|
||||
py_lcd_init0();
|
||||
py_fir_init0();
|
||||
py_tof_init0();
|
||||
|
||||
#if MICROPY_HW_ENABLE_RTC
|
||||
if (first_soft_reset) {
|
||||
|
||||
353
src/omv/py/py_tof.c
Normal file
353
src/omv/py/py_tof.c
Normal file
@ -0,0 +1,353 @@
|
||||
/*
|
||||
* This file is part of the OpenMV project.
|
||||
* Copyright (c) 2013/2014 Ibrahim Abdelkader <i.abdalkader@gmail.com>
|
||||
* This work is licensed under the MIT license, see the file LICENSE for details.
|
||||
*
|
||||
* ToF Module.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stm32f4xx_hal.h>
|
||||
|
||||
#include <mp.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
#include "fb_alloc.h"
|
||||
#include "xalloc.h"
|
||||
#include "py_assert.h"
|
||||
#include "py_image.h"
|
||||
#include "py_helper.h"
|
||||
#include "systick.h"
|
||||
#include "py_tof.h"
|
||||
|
||||
#define I2C_TIMEOUT (1000)
|
||||
#define TOF_SLAVE_ADDR (0x58<<1)
|
||||
|
||||
#define TOF_VDIN_PIN (GPIO_PIN_12)
|
||||
#define TOF_VDIN_PORT (GPIOD)
|
||||
|
||||
#define TOF_STDBY_PIN (GPIO_PIN_13)
|
||||
#define TOF_STDBY_PORT (GPIOD)
|
||||
|
||||
#define TOF_RESET_PIN (GPIO_PIN_5)
|
||||
#define TOF_RESET_PORT (GPIOA)
|
||||
|
||||
#define GPIO_READ(port, pin) HAL_GPIO_ReadPin(port, pin)
|
||||
#define GPIO_LOW(port, pin) HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET)
|
||||
#define GPIO_HIGH(port, pin) HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET)
|
||||
|
||||
static SPI_HandleTypeDef SPIHandle;
|
||||
static I2C_HandleTypeDef I2CHandle;
|
||||
|
||||
static void spi_config()
|
||||
{
|
||||
// Enable SPI clock
|
||||
__SPI2_CLK_ENABLE();
|
||||
|
||||
// SPI pins configuration
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
GPIO_InitStructure.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
|
||||
GPIO_InitStructure.Alternate = GPIO_AF5_SPI2;
|
||||
|
||||
GPIO_InitStructure.Pin = GPIO_PIN_12;
|
||||
HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
GPIO_InitStructure.Pin = GPIO_PIN_13;
|
||||
HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
GPIO_InitStructure.Pin = GPIO_PIN_15;
|
||||
HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
// SPI configuration
|
||||
SPIHandle.Instance = SPI2;
|
||||
SPIHandle.Init.Mode = SPI_MODE_SLAVE;
|
||||
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_HARD_INPUT;
|
||||
SPIHandle.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
|
||||
SPIHandle.Init.FirstBit = SPI_FIRSTBIT_LSB;
|
||||
SPIHandle.Init.TIMode = SPI_TIMODE_DISABLED;
|
||||
SPIHandle.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED;
|
||||
SPIHandle.Init.CRCPolynomial = 7;
|
||||
|
||||
// Initialize the SPI
|
||||
if (HAL_SPI_Init(&SPIHandle) != HAL_OK) {
|
||||
// Initialization error
|
||||
nlr_jump(mp_obj_new_exception_msg(&mp_type_RuntimeError, "ToF SPI init failed!!"));
|
||||
}
|
||||
}
|
||||
|
||||
static void i2c_init()
|
||||
{
|
||||
// Enable I2C clock
|
||||
__I2C2_CLK_ENABLE();
|
||||
|
||||
// I2C pins configuration
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
GPIO_InitStructure.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStructure.Speed = GPIO_SPEED_LOW;
|
||||
GPIO_InitStructure.Mode = GPIO_MODE_AF_OD;
|
||||
GPIO_InitStructure.Alternate = GPIO_AF4_I2C2;
|
||||
|
||||
GPIO_InitStructure.Pin = GPIO_PIN_10;
|
||||
HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
GPIO_InitStructure.Pin = GPIO_PIN_11;
|
||||
HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
|
||||
|
||||
// I2C configuration
|
||||
I2CHandle.Instance = I2C2;
|
||||
I2CHandle.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
|
||||
I2CHandle.Init.ClockSpeed = 10000;
|
||||
I2CHandle.Init.DualAddressMode = I2C_DUALADDRESS_DISABLED;
|
||||
I2CHandle.Init.DutyCycle = I2C_DUTYCYCLE_2;
|
||||
I2CHandle.Init.GeneralCallMode = I2C_GENERALCALL_DISABLED;
|
||||
I2CHandle.Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
|
||||
I2CHandle.Init.OwnAddress1 = 0xFE;
|
||||
I2CHandle.Init.OwnAddress2 = 0xFE;
|
||||
|
||||
// Initialize I2C
|
||||
if (HAL_I2C_Init(&I2CHandle) != HAL_OK) {
|
||||
// Initialization error
|
||||
nlr_jump(mp_obj_new_exception_msg(&mp_type_RuntimeError, "ToF I2C init failed!!"));
|
||||
}
|
||||
}
|
||||
|
||||
int i2c_write_bytes(uint8_t slv_addr, uint8_t *buf, int len, bool stop)
|
||||
{
|
||||
int ret=0;
|
||||
|
||||
__disable_irq();
|
||||
if(HAL_I2C_Master_Transmit(&I2CHandle, slv_addr, buf, len, I2C_TIMEOUT) != HAL_OK) {
|
||||
ret = -1;
|
||||
}
|
||||
__enable_irq();
|
||||
return ret;
|
||||
}
|
||||
|
||||
int i2c_read_bytes(uint8_t slv_addr, uint8_t mem_addr, uint8_t *buf, int len, bool stop)
|
||||
{
|
||||
int ret=0;
|
||||
|
||||
__disable_irq();
|
||||
if (HAL_I2C_Mem_Read(&I2CHandle, slv_addr, mem_addr, I2C_MEMADD_SIZE_8BIT, buf, len, I2C_TIMEOUT) != HAL_OK) {
|
||||
ret = -1;
|
||||
}
|
||||
__enable_irq();
|
||||
return ret;
|
||||
}
|
||||
|
||||
static mp_obj_t py_tof_deinit()
|
||||
{
|
||||
return mp_const_none;
|
||||
}
|
||||
|
||||
mp_obj_t py_tof_init(uint n_args, const mp_obj_t *args, mp_map_t *kw_args)
|
||||
{
|
||||
// Init I2C
|
||||
i2c_init();
|
||||
|
||||
// Init SPI
|
||||
spi_config();
|
||||
|
||||
// Configure GPIOs
|
||||
// VDIN pin
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
GPIO_InitStructure.Pin = TOF_VDIN_PIN;
|
||||
GPIO_InitStructure.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
|
||||
GPIO_LOW(TOF_VDIN_PORT, TOF_VDIN_PIN);
|
||||
HAL_GPIO_Init(TOF_VDIN_PORT, &GPIO_InitStructure);
|
||||
|
||||
// Standby
|
||||
GPIO_InitStructure.Pin = TOF_STDBY_PIN;
|
||||
GPIO_InitStructure.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
|
||||
GPIO_LOW(TOF_STDBY_PORT, TOF_STDBY_PIN);
|
||||
HAL_GPIO_Init(TOF_STDBY_PORT, &GPIO_InitStructure);
|
||||
|
||||
// Reset
|
||||
GPIO_InitStructure.Pin = TOF_RESET_PIN;
|
||||
GPIO_InitStructure.Pull = GPIO_PULLUP;
|
||||
GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
|
||||
GPIO_HIGH(TOF_RESET_PORT, TOF_RESET_PIN);
|
||||
HAL_GPIO_Init(TOF_RESET_PORT, &GPIO_InitStructure);
|
||||
|
||||
// Reset cycle
|
||||
GPIO_LOW(TOF_RESET_PORT, TOF_RESET_PIN);
|
||||
systick_sleep(10);
|
||||
GPIO_HIGH(TOF_RESET_PORT, TOF_RESET_PIN);
|
||||
|
||||
// Initialization
|
||||
// Enable timing generator
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x80, 0x01, 0x00, 0x00}, 4, true);
|
||||
// Disable standby mode
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x08, 0x00, 0x00, 0x00}, 4, true);
|
||||
systick_sleep(100);
|
||||
// Disable timing generator
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x80, 0x00, 0x00, 0x00}, 4, true);
|
||||
|
||||
// Set defaults INIT_0, INIT_1, INIT_2
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x12, 0x0A, 0x00, 0x00}, 4, true);
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x1B, 0x0A, 0x00, 0x00}, 4, true);
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x0B, 0x00, 0x20, 0x00}, 4, true);
|
||||
|
||||
// Config register (Update Select = 0x00, slave mode = 1)
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x81, 0x01, 0x00, 0x00}, 4, true);
|
||||
|
||||
// Internal illumination
|
||||
// Step = 5ma, Curr = 20 steps, Bias = 00 steps (100mA on, 0mA off)
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x39, 0x00, 0x00, 0x00}, 4, true);
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x3B, 0x00, 0x40, 0xFA}, 4, true);
|
||||
|
||||
// Enable dyn power, Re-arrange data
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x5C, 0x00, 0x10, 0x34}, 4, true);
|
||||
|
||||
// Enable shutter and easy conf
|
||||
//i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x5B, 0x00, 0x00, 0xC0}, 4, true);
|
||||
|
||||
// Enable test pattern
|
||||
//i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x6C, 0x44, 0xA4, 0x00}, 4, true);
|
||||
//i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0xD9, 0x0C, 0x40, 0x00}, 4, true);
|
||||
|
||||
// OP Mode: 1-lane SSI, OP_CLK(second byte) 24MHz = 0x00, 12MHz = 0x02, 6MHz = 0x04, 3MHz = 0x06
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0xDE, 0x10, 0x00, 0x00}, 4, true);
|
||||
|
||||
// Enable timing generator
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, (uint8_t [4]){0x80, 0x01, 0x00, 0x00}, 4, true);
|
||||
|
||||
//uint8_t buf[3];
|
||||
//i2c_read_bytes(TOF_SLAVE_ADDR, 0x6C, buf, 3, true);
|
||||
//printf("buf: %x %x %x\n", buf[0], buf[1], buf[2]);
|
||||
return mp_const_none;
|
||||
}
|
||||
|
||||
static mp_obj_t py_tof_write_reg(mp_obj_t addr, mp_obj_t vals_obj) {
|
||||
mp_obj_t *array;
|
||||
mp_obj_get_array_fixed_n(vals_obj, 3, &array);
|
||||
|
||||
uint8_t vals[4] = {
|
||||
mp_obj_get_int(addr),
|
||||
mp_obj_get_int(array[0]),
|
||||
mp_obj_get_int(array[1]),
|
||||
mp_obj_get_int(array[2])
|
||||
};
|
||||
|
||||
i2c_write_bytes(TOF_SLAVE_ADDR, vals, 4, true);
|
||||
return mp_const_none;
|
||||
}
|
||||
|
||||
static void read_frame(uint8_t *buf, uint32_t buf_size)
|
||||
{
|
||||
// Pulse VDIN
|
||||
GPIO_HIGH(TOF_VDIN_PORT, TOF_VDIN_PIN);
|
||||
systick_sleep(1);
|
||||
GPIO_HIGH(TOF_VDIN_PORT, TOF_VDIN_PIN);
|
||||
|
||||
__disable_irq();
|
||||
//do {
|
||||
// if (HAL_SPI_Receive(&SPIHandle, buf, 1, 100000) != HAL_OK) {
|
||||
// nlr_jump(mp_obj_new_exception_msg(&mp_type_RuntimeError, "ToF error reading frame!!"));
|
||||
// }
|
||||
//} while (buf[0] != 0xFF);
|
||||
|
||||
do {
|
||||
if (HAL_SPI_Receive(&SPIHandle, buf, 1, 100000) != HAL_OK) {
|
||||
nlr_jump(mp_obj_new_exception_msg(&mp_type_RuntimeError, "ToF error reading frame!!"));
|
||||
}
|
||||
} while (buf[0] == 0xFF);
|
||||
|
||||
if (HAL_SPI_Receive(&SPIHandle, buf+1, buf_size-1, 100000) != HAL_OK) {
|
||||
nlr_jump(mp_obj_new_exception_msg(&mp_type_RuntimeError, "ToF error reading frame!!"));
|
||||
}
|
||||
|
||||
__enable_irq();
|
||||
}
|
||||
|
||||
mp_obj_t py_tof_read_frame()
|
||||
{
|
||||
int buf_size = 80*60*4;
|
||||
uint8_t *buf = fb_alloc0(buf_size);
|
||||
|
||||
read_frame(buf, buf_size);
|
||||
|
||||
uint16_t *pix =(uint16_t*) buf;
|
||||
for (int y=0; y<6; y++) {
|
||||
for (int x=0; x<10; x++, pix+=2) {
|
||||
// Phase
|
||||
uint32_t p;
|
||||
p = (uint32_t)pix[1] & 0xFFF;
|
||||
printf("%lu,", p);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
fb_free();
|
||||
return mp_const_none;
|
||||
}
|
||||
|
||||
mp_obj_t py_tof_draw_frame(uint n_args, const mp_obj_t *args, mp_map_t *kw_args)
|
||||
{
|
||||
int buf_size = 80*60*4;
|
||||
uint8_t *buf = fb_alloc0(buf_size);
|
||||
|
||||
image_t *arg_img = py_image_cobj(args[0]);
|
||||
PY_ASSERT_FALSE_MSG(IM_IS_JPEG(arg_img), "Operation not supported on JPEG");
|
||||
int draw_phase = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_phase), 0);
|
||||
|
||||
read_frame(buf, buf_size);
|
||||
|
||||
uint16_t *pix =(uint16_t*) buf;
|
||||
for (int y=0; y<60; y++) {
|
||||
for (int x=0; x<80; x++, pix+=2) {
|
||||
// Phase
|
||||
uint32_t p;
|
||||
|
||||
if (draw_phase) {
|
||||
p = (uint32_t)pix[1] & 0xFFF;
|
||||
} else {
|
||||
p = (uint32_t)pix[0] & 0xFFF;
|
||||
}
|
||||
IM_SET_GS_PIXEL(arg_img, x, y, (p*255)/4095);
|
||||
|
||||
// Ambient
|
||||
//IM_SET_GS_PIXEL(arg_img, x, y, (buf[3]>>4)*255/15);
|
||||
}
|
||||
}
|
||||
fb_free();
|
||||
return mp_const_none;
|
||||
}
|
||||
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_init_obj, 0, py_tof_init);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_0(py_tof_deinit_obj, py_tof_deinit);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_0(py_tof_read_frame_obj, py_tof_read_frame);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_draw_frame_obj, 1, py_tof_draw_frame);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_2(py_tof_write_reg_obj, py_tof_write_reg);
|
||||
static const mp_map_elem_t globals_dict_table[] = {
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR___name__), MP_OBJ_NEW_QSTR(MP_QSTR_tof) },
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR_init), (mp_obj_t)&py_tof_init_obj },
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR_deinit), (mp_obj_t)&py_tof_deinit_obj },
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR_read_frame), (mp_obj_t)&py_tof_read_frame_obj },
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR_draw_frame), (mp_obj_t)&py_tof_draw_frame_obj },
|
||||
{ MP_OBJ_NEW_QSTR(MP_QSTR___write_reg), (mp_obj_t)&py_tof_write_reg_obj },
|
||||
{ NULL, NULL },
|
||||
};
|
||||
STATIC MP_DEFINE_CONST_DICT(globals_dict, globals_dict_table);
|
||||
|
||||
const mp_obj_module_t tof_module = {
|
||||
.base = { &mp_type_module },
|
||||
.name = MP_QSTR_tof,
|
||||
.globals = (mp_obj_t)&globals_dict,
|
||||
};
|
||||
|
||||
void py_tof_init0()
|
||||
{
|
||||
}
|
||||
12
src/omv/py/py_tof.h
Normal file
12
src/omv/py/py_tof.h
Normal file
@ -0,0 +1,12 @@
|
||||
/*
|
||||
* This file is part of the OpenMV project.
|
||||
* Copyright (c) 2013/2014 Ibrahim Abdelkader <i.abdalkader@gmail.com>
|
||||
* This work is licensed under the MIT license, see the file LICENSE for details.
|
||||
*
|
||||
* ToF Python module.
|
||||
*
|
||||
*/
|
||||
#ifndef __PY_TOF_H__
|
||||
#define __PY_TOF_H__
|
||||
void py_tof_init0();
|
||||
#endif // __PY_TOF_H__
|
||||
@ -287,3 +287,9 @@ Q(ssid)
|
||||
Q(key)
|
||||
Q(security)
|
||||
Q(bssid)
|
||||
|
||||
// ToF Module
|
||||
Q(tof)
|
||||
Q(read_frame)
|
||||
Q(draw_frame)
|
||||
Q(phase)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user