modules/py_tof: Add support for VL53L8CX.

Signed-off-by: iabdalkader <i.abdalkader@gmail.com>
This commit is contained in:
iabdalkader 2025-03-08 11:47:44 +01:00
parent 35233d1650
commit 5dfcfce74d

View File

@ -36,33 +36,41 @@
#include "py_image.h" #include "py_image.h"
#include "framebuffer.h" #include "framebuffer.h"
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53L5CX_ENABLE
#include "vl53l5cx_api.h" #include "vl53l5cx_api.h"
#endif #endif
#define VL53L5CX_ADDR 0x52 #if OMV_TOF_VL53L8CX_ENABLE
#define VL53L5CX_WIDTH 8 #include "vl53l8cx_api.h"
#define VL53L5CX_HEIGHT 8 #endif
#define VL53L5CX_FRAME_DATA_SIZE 64
typedef enum tof_type { #if OMV_TOF_VL53L5CX_ENABLE || OMV_TOF_VL53L8CX_ENABLE
TOF_NONE, #define OMV_TOF_VL53LX_ENABLE (1)
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #endif
TOF_VL53L5CX,
#define OMV_TOF_VL53LX_ADDR 0x52
#define OMV_TOF_VL53LX_WIDTH 8
#define OMV_TOF_VL53LX_HEIGHT 8
typedef enum omv_tof_id {
OMV_TOF_NONE,
#if OMV_TOF_VL53LX_ENABLE
OMV_TOF_VL53LX_ID,
#endif #endif
} tof_type_t; } omv_tof_id_t;
static int tof_width = 0; static int tof_width = 0;
static int tof_height = 0; static int tof_height = 0;
static bool tof_transposed = false; static bool tof_transposed = false;
static tof_type_t tof_sensor = TOF_NONE; static omv_tof_id_t tof_sensor = OMV_TOF_NONE;
static omv_i2c_t tof_bus = { 0 }; static omv_i2c_t tof_bus = { 0 };
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
static VL53L5CX_Configuration vl53l5cx_dev = { static vl53lx_dev_t vl53lx_dev = {
.platform = { .platform = {
.bus = &tof_bus, .bus = &tof_bus,
.address = VL53L5CX_ADDR, .address = OMV_TOF_VL53LX_ADDR,
} }
}; };
#endif #endif
@ -97,27 +105,27 @@ static void tof_fill_image_float_obj(image_t *img, mp_obj_t *data, float min, fl
} }
} }
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
static void tof_vl53l5cx_get_depth(VL53L5CX_Configuration *vl53l5cx_dev, float *frame, uint32_t timeout) { static void tof_vl53lx_get_depth(vl53lx_dev_t *vl53lx_dev, float *frame, uint32_t timeout) {
uint8_t frame_ready = 0; uint8_t frame_ready = 0;
// Note depending on the config in platform.h, this struct can be too big to alloc on the stack. // Note depending on the config in platform.h, this struct can be too big to alloc on the stack.
VL53L5CX_ResultsData ranging_data; vl53lx_data_t ranging_data;
for (mp_uint_t start = mp_hal_ticks_ms(); !frame_ready; mp_hal_delay_ms(1)) { for (mp_uint_t start = mp_hal_ticks_ms(); !frame_ready; mp_hal_delay_ms(1)) {
if (vl53l5cx_check_data_ready(vl53l5cx_dev, &frame_ready) != 0) { if (vl53lx_check_data_ready(vl53lx_dev, &frame_ready) != 0) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53L5CX ranging failed")); mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53LX ranging failed"));
} }
if ((mp_hal_ticks_ms() - start) >= timeout) { if ((mp_hal_ticks_ms() - start) >= timeout) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53L5CX ranging timeout")); mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53LX ranging timeout"));
} }
} }
if (vl53l5cx_get_ranging_data(vl53l5cx_dev, &ranging_data) != 0) { if (vl53lx_get_ranging_data(vl53lx_dev, &ranging_data) != 0) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53L5CX ranging failed")); mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53LX ranging failed"));
} }
for (int i = 0, ii = VL53L5CX_WIDTH * VL53L5CX_HEIGHT; i < ii; i++) { for (int i = 0, ii = OMV_TOF_VL53LX_WIDTH * OMV_TOF_VL53LX_HEIGHT; i < ii; i++) {
frame[i] = (float) ranging_data.distance_mm[i]; frame[i] = (float) ranging_data.distance_mm[i];
} }
} }
@ -202,17 +210,17 @@ static mp_obj_t py_tof_reset() {
tof_height = 0; tof_height = 0;
tof_transposed = false; tof_transposed = false;
if (tof_sensor != TOF_NONE) { if (tof_sensor != OMV_TOF_NONE) {
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
if (tof_sensor == TOF_VL53L5CX) { if (tof_sensor == OMV_TOF_VL53LX_ID) {
vl53l5cx_stop_ranging(&vl53l5cx_dev); vl53lx_stop_ranging(&vl53lx_dev);
} }
#endif #endif
omv_i2c_deinit(&tof_bus); omv_i2c_deinit(&tof_bus);
tof_sensor = TOF_NONE; tof_sensor = OMV_TOF_NONE;
} }
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
vl53l5cx_reset(&vl53l5cx_dev.platform); vl53lx_reset(&vl53lx_dev.platform);
#endif #endif
return mp_const_none; return mp_const_none;
} }
@ -222,8 +230,8 @@ static mp_obj_t py_tof_deinit() {
tof_width = 0; tof_width = 0;
tof_height = 0; tof_height = 0;
tof_transposed = false; tof_transposed = false;
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
vl53l5cx_shutdown(&vl53l5cx_dev.platform); vl53lx_shutdown(&vl53lx_dev.platform);
#endif #endif
return mp_const_none; return mp_const_none;
} }
@ -251,9 +259,9 @@ mp_obj_t py_tof_init(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args)
int dev_size = omv_i2c_scan(&tof_bus, dev_list, sizeof(dev_list)); int dev_size = omv_i2c_scan(&tof_bus, dev_list, sizeof(dev_list));
for (int i = 0; i < dev_size && type == -1; i++) { for (int i = 0; i < dev_size && type == -1; i++) {
switch (dev_list[i]) { switch (dev_list[i]) {
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
case (VL53L5CX_ADDR): { case (OMV_TOF_VL53LX_ADDR): {
type = TOF_VL53L5CX; type = OMV_TOF_VL53LX_ID;
break; break;
} }
#endif #endif
@ -275,50 +283,50 @@ mp_obj_t py_tof_init(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args)
// Initialize the detected sensor. // Initialize the detected sensor.
first_init = true; first_init = true;
switch (type) { switch (type) {
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
case TOF_VL53L5CX: { case OMV_TOF_VL53LX_ID: {
int error = 0; int error = 0;
uint8_t isAlive = 0; uint8_t isAlive = 0;
TOF_VL53L5CX_RETRY: TOF_VL53LX_RETRY:
// Initialize I2C bus. // Initialize I2C bus.
omv_i2c_init(&tof_bus, OMV_TOF_I2C_ID, OMV_TOF_I2C_SPEED); omv_i2c_init(&tof_bus, OMV_TOF_I2C_ID, OMV_TOF_I2C_SPEED);
// Check sensor and initialize. // Check sensor and initialize.
error |= vl53l5cx_is_alive(&vl53l5cx_dev, &isAlive); error |= vl53lx_is_alive(&vl53lx_dev, &isAlive);
error |= vl53l5cx_init(&vl53l5cx_dev); error |= vl53lx_init(&vl53lx_dev);
// Set resolution (number of zones). // Set resolution (number of zones).
// NOTE: This function must be called before updating the ranging frequency. // NOTE: This function must be called before updating the ranging frequency.
error |= vl53l5cx_set_resolution(&vl53l5cx_dev, VL53L5CX_RESOLUTION_8X8); error |= vl53lx_set_resolution(&vl53lx_dev, VL53LX_RESOLUTION_8X8);
// Set ranging frequency (FPS). // Set ranging frequency (FPS).
// For 4x4 the allowed ranging frequency range is 1 -> 60. // For 4x4 the allowed ranging frequency range is 1 -> 60.
// For 8x8 the allowed ranging frequency range is 1 -> 15. // For 8x8 the allowed ranging frequency range is 1 -> 15.
error |= vl53l5cx_set_ranging_frequency_hz(&vl53l5cx_dev, 15); error |= vl53lx_set_ranging_frequency_hz(&vl53lx_dev, 15);
// Set ranging mode to continuous: // Set ranging mode to continuous:
// The device continuously grabs frames with the set ranging frequency. // The device continuously grabs frames with the set ranging frequency.
// Maximum ranging depth and ambient immunity are better. // Maximum ranging depth and ambient immunity are better.
// This mode is advised for fast ranging measurements or high performances. // This mode is advised for fast ranging measurements or high performances.
error |= vl53l5cx_set_ranging_mode(&vl53l5cx_dev, VL53L5CX_RANGING_MODE_CONTINUOUS); error |= vl53lx_set_ranging_mode(&vl53lx_dev, VL53LX_RANGING_MODE_CONTINUOUS);
error |= vl53l5cx_set_sharpener_percent(&vl53l5cx_dev, 50); error |= vl53lx_set_sharpener_percent(&vl53lx_dev, 50);
// Start ranging. // Start ranging.
error |= vl53l5cx_start_ranging(&vl53l5cx_dev); error |= vl53lx_start_ranging(&vl53lx_dev);
if (error != 0 && first_init) { if (error != 0 && first_init) {
first_init = false; first_init = false;
// Recover bus and scan one more time. // Recover bus and scan one more time.
omv_i2c_pulse_scl(&tof_bus); omv_i2c_pulse_scl(&tof_bus);
goto TOF_VL53L5CX_RETRY; goto TOF_VL53LX_RETRY;
} else if (error != 0) { } else if (error != 0) {
py_tof_reset(); py_tof_reset();
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("Failed to init the VL53L5CX")); mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("Failed to init the VL53LX"));
} }
tof_sensor = TOF_VL53L5CX; tof_sensor = OMV_TOF_VL53LX_ID;
tof_width = VL53L5CX_WIDTH; tof_width = OMV_TOF_VL53LX_WIDTH;
tof_height = VL53L5CX_HEIGHT; tof_height = OMV_TOF_VL53LX_HEIGHT;
break; break;
} }
#endif #endif
@ -332,7 +340,7 @@ mp_obj_t py_tof_init(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args)
static MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_init_obj, 0, py_tof_init); static MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_init_obj, 0, py_tof_init);
static mp_obj_t py_tof_type() { static mp_obj_t py_tof_type() {
if (tof_sensor != TOF_NONE) { if (tof_sensor != OMV_TOF_NONE) {
return mp_obj_new_int(tof_sensor); return mp_obj_new_int(tof_sensor);
} }
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized")); mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
@ -340,7 +348,7 @@ static mp_obj_t py_tof_type() {
static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_type_obj, py_tof_type); static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_type_obj, py_tof_type);
static mp_obj_t py_tof_width() { static mp_obj_t py_tof_width() {
if (tof_sensor != TOF_NONE) { if (tof_sensor != OMV_TOF_NONE) {
return mp_obj_new_int(tof_width); return mp_obj_new_int(tof_width);
} }
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized")); mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
@ -348,7 +356,7 @@ static mp_obj_t py_tof_width() {
static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_width_obj, py_tof_width); static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_width_obj, py_tof_width);
static mp_obj_t py_tof_height() { static mp_obj_t py_tof_height() {
if (tof_sensor != TOF_NONE) { if (tof_sensor != OMV_TOF_NONE) {
return mp_obj_new_int(tof_height); return mp_obj_new_int(tof_height);
} }
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized")); mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
@ -357,8 +365,8 @@ static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_height_obj, py_tof_height);
static mp_obj_t py_tof_refresh() { static mp_obj_t py_tof_refresh() {
switch (tof_sensor) { switch (tof_sensor) {
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
case TOF_VL53L5CX: case OMV_TOF_VL53LX_ID:
return mp_obj_new_int(15); return mp_obj_new_int(15);
#endif #endif
default: default:
@ -383,13 +391,15 @@ mp_obj_t py_tof_read_depth(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw
tof_transposed = args[ARG_transpose].u_bool; tof_transposed = args[ARG_transpose].u_bool;
switch (tof_sensor) { switch (tof_sensor) {
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
case TOF_VL53L5CX: { case OMV_TOF_VL53LX_ID: {
fb_alloc_mark(); fb_alloc_mark();
float *frame = fb_alloc(VL53L5CX_WIDTH * VL53L5CX_HEIGHT * sizeof(float), FB_ALLOC_PREFER_SPEED); float *frame = fb_alloc(OMV_TOF_VL53LX_WIDTH * OMV_TOF_VL53LX_HEIGHT * sizeof(float),
tof_vl53l5cx_get_depth(&vl53l5cx_dev, frame, args[ARG_timeout].u_int); FB_ALLOC_PREFER_SPEED);
mp_obj_t result = tof_get_depth_obj(VL53L5CX_WIDTH, VL53L5CX_HEIGHT, frame, !args[ARG_hmirror].u_bool, tof_vl53lx_get_depth(&vl53lx_dev, frame, args[ARG_timeout].u_int);
args[ARG_vflip].u_bool, args[ARG_transpose].u_bool, true); mp_obj_t result = tof_get_depth_obj(OMV_TOF_VL53LX_WIDTH, OMV_TOF_VL53LX_HEIGHT, frame,
!args[ARG_hmirror].u_bool, args[ARG_vflip].u_bool,
args[ARG_transpose].u_bool, true);
fb_alloc_free_till_mark(); fb_alloc_free_till_mark();
return result; return result;
} }
@ -426,7 +436,7 @@ mp_obj_t py_tof_draw_depth(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw
mp_arg_parse_all(n_args - 2, pos_args + 2, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args); mp_arg_parse_all(n_args - 2, pos_args + 2, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
// Sanity checks // Sanity checks
if (tof_sensor == TOF_NONE) { if (tof_sensor == OMV_TOF_NONE) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized")); mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
} }
@ -553,14 +563,16 @@ mp_obj_t py_tof_snapshot(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_a
src_img.data = fb_alloc(src_img.w * src_img.h * sizeof(uint8_t), FB_ALLOC_NO_HINT); src_img.data = fb_alloc(src_img.w * src_img.h * sizeof(uint8_t), FB_ALLOC_NO_HINT);
switch (tof_sensor) { switch (tof_sensor) {
#if (OMV_TOF_VL53L5CX_ENABLE == 1) #if OMV_TOF_VL53LX_ENABLE
case TOF_VL53L5CX: { case OMV_TOF_VL53LX_ID: {
float *frame = fb_alloc(VL53L5CX_WIDTH * VL53L5CX_HEIGHT * sizeof(float), FB_ALLOC_PREFER_SPEED); float *frame = fb_alloc(OMV_TOF_VL53LX_WIDTH * OMV_TOF_VL53LX_HEIGHT * sizeof(float),
tof_vl53l5cx_get_depth(&vl53l5cx_dev, frame, args[ARG_timeout].u_int); FB_ALLOC_PREFER_SPEED);
tof_vl53lx_get_depth(&vl53lx_dev, frame, args[ARG_timeout].u_int);
if (args[ARG_scale].u_obj == mp_const_none) { if (args[ARG_scale].u_obj == mp_const_none) {
fast_get_min_max(frame, VL53L5CX_WIDTH * VL53L5CX_HEIGHT, &min, &max); fast_get_min_max(frame, OMV_TOF_VL53LX_WIDTH * OMV_TOF_VL53LX_HEIGHT, &min, &max);
} }
imlib_fill_image_from_float(&src_img, VL53L5CX_WIDTH, VL53L5CX_HEIGHT, frame, min, max, imlib_fill_image_from_float(&src_img, OMV_TOF_VL53LX_WIDTH, OMV_TOF_VL53LX_HEIGHT,
frame, min, max,
!args[ARG_hmirror].u_bool, args[ARG_vflip].u_bool, !args[ARG_hmirror].u_bool, args[ARG_vflip].u_bool,
args[ARG_transpose].u_bool, true); args[ARG_transpose].u_bool, true);
break; break;
@ -584,21 +596,20 @@ mp_obj_t py_tof_snapshot(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_a
static MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_snapshot_obj, 0, py_tof_snapshot); static MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_snapshot_obj, 0, py_tof_snapshot);
static const mp_rom_map_elem_t globals_dict_table[] = { static const mp_rom_map_elem_t globals_dict_table[] = {
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tof) }, { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tof) },
{ MP_ROM_QSTR(MP_QSTR_TOF_NONE), MP_ROM_INT(TOF_NONE) }, #if OMV_TOF_VL53LX_ENABLE
#if (OMV_TOF_VL53L5CX_ENABLE == 1) { MP_ROM_QSTR(MP_QSTR_TOF_VL53LX), MP_ROM_INT(OMV_TOF_VL53LX_ID) },
{ MP_ROM_QSTR(MP_QSTR_TOF_VL53L5CX), MP_ROM_INT(TOF_VL53L5CX) },
#endif #endif
{ MP_ROM_QSTR(MP_QSTR_init), MP_ROM_PTR(&py_tof_init_obj) }, { MP_ROM_QSTR(MP_QSTR_init), MP_ROM_PTR(&py_tof_init_obj) },
{ MP_ROM_QSTR(MP_QSTR_reset), MP_ROM_PTR(&py_tof_reset_obj) }, { MP_ROM_QSTR(MP_QSTR_reset), MP_ROM_PTR(&py_tof_reset_obj) },
{ MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&py_tof_deinit_obj) }, { MP_ROM_QSTR(MP_QSTR_deinit), MP_ROM_PTR(&py_tof_deinit_obj) },
{ MP_ROM_QSTR(MP_QSTR_type), MP_ROM_PTR(&py_tof_type_obj) }, { MP_ROM_QSTR(MP_QSTR_type), MP_ROM_PTR(&py_tof_type_obj) },
{ MP_ROM_QSTR(MP_QSTR_width), MP_ROM_PTR(&py_tof_width_obj) }, { MP_ROM_QSTR(MP_QSTR_width), MP_ROM_PTR(&py_tof_width_obj) },
{ MP_ROM_QSTR(MP_QSTR_height), MP_ROM_PTR(&py_tof_height_obj) }, { MP_ROM_QSTR(MP_QSTR_height), MP_ROM_PTR(&py_tof_height_obj) },
{ MP_ROM_QSTR(MP_QSTR_refresh), MP_ROM_PTR(&py_tof_refresh_obj) }, { MP_ROM_QSTR(MP_QSTR_refresh), MP_ROM_PTR(&py_tof_refresh_obj) },
{ MP_ROM_QSTR(MP_QSTR_read_depth), MP_ROM_PTR(&py_tof_read_depth_obj) }, { MP_ROM_QSTR(MP_QSTR_read_depth), MP_ROM_PTR(&py_tof_read_depth_obj) },
{ MP_ROM_QSTR(MP_QSTR_draw_depth), MP_ROM_PTR(&py_tof_draw_depth_obj) }, { MP_ROM_QSTR(MP_QSTR_draw_depth), MP_ROM_PTR(&py_tof_draw_depth_obj) },
{ MP_ROM_QSTR(MP_QSTR_snapshot), MP_ROM_PTR(&py_tof_snapshot_obj) } { MP_ROM_QSTR(MP_QSTR_snapshot), MP_ROM_PTR(&py_tof_snapshot_obj) }
}; };
static MP_DEFINE_CONST_DICT(globals_dict, globals_dict_table); static MP_DEFINE_CONST_DICT(globals_dict, globals_dict_table);