openmv/modules/py_tof.c
Kwabena W. Agyeman e14bbe586d lib/imlib: Fix update_jpeg_buffer to update from the passed image.
framebuffer_update_jpeg_buffer was previously bugged as it always
updated the jpeg buffer from the frame buffer versus the image
object it was attached to. e.g. img.flush() always flushed the
frame buffer and not the image object it was called on.
2025-07-07 20:39:29 -07:00

624 lines
23 KiB
C

/*
* SPDX-License-Identifier: MIT
*
* Copyright (C) 2013-2024 OpenMV, LLC.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*
* Python module for time of flight sensors.
*/
#include "py/runtime.h"
#include "py/objlist.h"
#include "py/mphal.h"
#include "omv_boardconfig.h"
#if (MICROPY_PY_TOF == 1)
#include "omv_i2c.h"
#include "py_assert.h"
#include "py_helper.h"
#include "py_image.h"
#include "framebuffer.h"
#if OMV_TOF_VL53L5CX_ENABLE
#include "vl53l5cx_api.h"
#endif
#if OMV_TOF_VL53L8CX_ENABLE
#include "vl53l8cx_api.h"
#endif
#if OMV_TOF_VL53L5CX_ENABLE || OMV_TOF_VL53L8CX_ENABLE
#define OMV_TOF_VL53LX_ENABLE (1)
#endif
#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
} omv_tof_id_t;
static int tof_width = 0;
static int tof_height = 0;
static bool tof_transposed = false;
static omv_tof_id_t tof_sensor = OMV_TOF_NONE;
static omv_i2c_t tof_bus = { 0 };
#if OMV_TOF_VL53LX_ENABLE
static vl53lx_dev_t vl53lx_dev = {
.platform = {
.bus = &tof_bus,
.address = OMV_TOF_VL53LX_ADDR,
}
};
#endif
// img->w == data_w && img->h == data_h && img->pixfmt == PIXFORMAT_GRAYSCALE
static void tof_fill_image_float_obj(image_t *img, mp_obj_t *data, float min, float max) {
float tmp = min;
min = (min < max) ? min : max;
max = (max > tmp) ? max : tmp;
float diff = 255.f / (max - min);
for (int y = 0; y < img->h; y++) {
int row_offset = y * img->w;
mp_obj_t *raw_row = data + row_offset;
uint8_t *row_pointer = ((uint8_t *) img->data) + row_offset;
for (int x = 0; x < img->w; x++) {
float raw = mp_obj_get_float(raw_row[x]);
if (raw < min) {
raw = min;
}
if (raw > max) {
raw = max;
}
int pixel = fast_roundf((raw - min) * diff);
row_pointer[x] = __USAT(pixel, 8);
}
}
}
#if OMV_TOF_VL53LX_ENABLE
static void tof_vl53lx_get_depth(vl53lx_dev_t *vl53lx_dev, float *frame, uint32_t timeout) {
uint8_t frame_ready = 0;
// Note depending on the config in platform.h, this struct can be too big to alloc on the stack.
vl53lx_data_t ranging_data;
for (mp_uint_t start = mp_hal_ticks_ms(); !frame_ready; mp_hal_delay_ms(1)) {
if (vl53lx_check_data_ready(vl53lx_dev, &frame_ready) != 0) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53LX ranging failed"));
}
if ((mp_hal_ticks_ms() - start) >= timeout) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53LX ranging timeout"));
}
}
if (vl53lx_get_ranging_data(vl53lx_dev, &ranging_data) != 0) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("VL53LX ranging failed"));
}
for (int i = 0, ii = OMV_TOF_VL53LX_WIDTH * OMV_TOF_VL53LX_HEIGHT; i < ii; i++) {
frame[i] = (float) ranging_data.distance_mm[i];
}
}
static mp_obj_t tof_get_depth_obj(int w, int h, float *frame, bool mirror,
bool flip, bool dst_transpose, bool src_transpose) {
mp_obj_list_t *list = (mp_obj_list_t *) mp_obj_new_list(w * h, NULL);
float min = FLT_MAX;
float max = -FLT_MAX;
int w_1 = w - 1;
int h_1 = h - 1;
if (!src_transpose) {
for (int y = 0; y < h; y++) {
int y_dst = flip ? (h_1 - y) : y;
float *raw_row = frame + (y * w);
mp_obj_t *list_row = list->items + (y_dst * w);
mp_obj_t *t_list_row = list->items + y_dst;
for (int x = 0; x < w; x++) {
int x_dst = mirror ? (w_1 - x) : x;
float raw = raw_row[x];
if (raw < min) {
min = raw;
}
if (raw > max) {
max = raw;
}
mp_obj_t f = mp_obj_new_float(raw);
if (!dst_transpose) {
list_row[x_dst] = f;
} else {
t_list_row[x_dst * h] = f;
}
}
}
} else {
for (int x = 0; x < w; x++) {
int x_dst = mirror ? (w_1 - x) : x;
float *raw_row = frame + (x * h);
mp_obj_t *t_list_row = list->items + (x_dst * h);
mp_obj_t *list_row = list->items + x_dst;
for (int y = 0; y < h; y++) {
int y_dst = flip ? (h_1 - y) : y;
float raw = raw_row[y];
if (raw < min) {
min = raw;
}
if (raw > max) {
max = raw;
}
mp_obj_t f = mp_obj_new_float(raw);
if (!dst_transpose) {
list_row[y_dst * w] = f;
} else {
t_list_row[y_dst] = f;
}
}
}
}
mp_obj_t tuple[3] = {
MP_OBJ_FROM_PTR(list),
mp_obj_new_float(min),
mp_obj_new_float(max)
};
return mp_obj_new_tuple(3, tuple);
}
#endif
static mp_obj_t py_tof_reset() {
tof_width = 0;
tof_height = 0;
tof_transposed = false;
if (tof_sensor != OMV_TOF_NONE) {
#if OMV_TOF_VL53LX_ENABLE
if (tof_sensor == OMV_TOF_VL53LX_ID) {
vl53lx_stop_ranging(&vl53lx_dev);
}
#endif
omv_i2c_deinit(&tof_bus);
tof_sensor = OMV_TOF_NONE;
}
#if OMV_TOF_VL53LX_ENABLE
vl53lx_reset(&vl53lx_dev.platform);
#endif
return mp_const_none;
}
static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_reset_obj, py_tof_reset);
static mp_obj_t py_tof_deinit() {
tof_width = 0;
tof_height = 0;
tof_transposed = false;
#if OMV_TOF_VL53LX_ENABLE
vl53lx_shutdown(&vl53lx_dev.platform);
#endif
return mp_const_none;
}
static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_deinit_obj, py_tof_deinit);
mp_obj_t py_tof_init(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_type };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_type, MP_ARG_INT, {.u_int = -1 } },
};
// Parse args.
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
py_tof_reset();
bool first_init = true;
int type = args[ARG_type].u_int;
if (type == -1) {
TOF_SCAN_RETRY:
omv_i2c_init(&tof_bus, OMV_TOF_I2C_ID, OMV_TOF_I2C_SPEED);
// Scan and detect any supported sensor.
uint8_t dev_list[10];
int dev_size = omv_i2c_scan(&tof_bus, dev_list, sizeof(dev_list));
for (int i = 0; i < dev_size && type == -1; i++) {
switch (dev_list[i]) {
#if OMV_TOF_VL53LX_ENABLE
case (OMV_TOF_VL53LX_ADDR): {
type = OMV_TOF_VL53LX_ID;
break;
}
#endif
default:
continue;
}
}
if (type == -1 && first_init) {
first_init = false;
// Recover bus and scan one more time.
omv_i2c_pulse_scl(&tof_bus);
goto TOF_SCAN_RETRY;
}
omv_i2c_deinit(&tof_bus);
}
// Initialize the detected sensor.
first_init = true;
switch (type) {
#if OMV_TOF_VL53LX_ENABLE
case OMV_TOF_VL53LX_ID: {
int error = 0;
uint8_t isAlive = 0;
TOF_VL53LX_RETRY:
// Initialize I2C bus.
omv_i2c_init(&tof_bus, OMV_TOF_I2C_ID, OMV_TOF_I2C_SPEED);
// Check sensor and initialize.
error |= vl53lx_is_alive(&vl53lx_dev, &isAlive);
error |= vl53lx_init(&vl53lx_dev);
// Set resolution (number of zones).
// NOTE: This function must be called before updating the ranging frequency.
error |= vl53lx_set_resolution(&vl53lx_dev, VL53LX_RESOLUTION_8X8);
// Set ranging frequency (FPS).
// For 4x4 the allowed ranging frequency range is 1 -> 60.
// For 8x8 the allowed ranging frequency range is 1 -> 15.
error |= vl53lx_set_ranging_frequency_hz(&vl53lx_dev, 15);
// Set ranging mode to continuous:
// The device continuously grabs frames with the set ranging frequency.
// Maximum ranging depth and ambient immunity are better.
// This mode is advised for fast ranging measurements or high performances.
error |= vl53lx_set_ranging_mode(&vl53lx_dev, VL53LX_RANGING_MODE_CONTINUOUS);
error |= vl53lx_set_sharpener_percent(&vl53lx_dev, 50);
// Start ranging.
error |= vl53lx_start_ranging(&vl53lx_dev);
if (error != 0 && first_init) {
first_init = false;
// Recover bus and scan one more time.
omv_i2c_pulse_scl(&tof_bus);
goto TOF_VL53LX_RETRY;
} else if (error != 0) {
py_tof_reset();
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("Failed to init the VL53LX"));
}
tof_sensor = OMV_TOF_VL53LX_ID;
tof_width = OMV_TOF_VL53LX_WIDTH;
tof_height = OMV_TOF_VL53LX_HEIGHT;
break;
}
#endif
default: {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("Failed to detect a supported TOF sensor."));
}
}
return mp_const_none;
}
static MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_init_obj, 0, py_tof_init);
static mp_obj_t py_tof_type() {
if (tof_sensor != OMV_TOF_NONE) {
return mp_obj_new_int(tof_sensor);
}
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
}
static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_type_obj, py_tof_type);
static mp_obj_t py_tof_width() {
if (tof_sensor != OMV_TOF_NONE) {
return mp_obj_new_int(tof_width);
}
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
}
static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_width_obj, py_tof_width);
static mp_obj_t py_tof_height() {
if (tof_sensor != OMV_TOF_NONE) {
return mp_obj_new_int(tof_height);
}
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
}
static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_height_obj, py_tof_height);
static mp_obj_t py_tof_refresh() {
switch (tof_sensor) {
#if OMV_TOF_VL53LX_ENABLE
case OMV_TOF_VL53LX_ID:
return mp_obj_new_int(15);
#endif
default:
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
}
}
static MP_DEFINE_CONST_FUN_OBJ_0(py_tof_refresh_obj, py_tof_refresh);
mp_obj_t py_tof_read_depth(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum { ARG_hmirror, ARG_vflip, ARG_transpose, ARG_timeout };
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_hmirror, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_bool = false } },
{ MP_QSTR_vflip, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_bool = false } },
{ MP_QSTR_transpose, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_bool = false } },
{ MP_QSTR_timeout, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = -1 } },
};
// Parse args.
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
tof_transposed = args[ARG_transpose].u_bool;
switch (tof_sensor) {
#if OMV_TOF_VL53LX_ENABLE
case OMV_TOF_VL53LX_ID: {
fb_alloc_mark();
float *frame = fb_alloc(OMV_TOF_VL53LX_WIDTH * OMV_TOF_VL53LX_HEIGHT * sizeof(float),
FB_ALLOC_PREFER_SPEED);
tof_vl53lx_get_depth(&vl53lx_dev, frame, args[ARG_timeout].u_int);
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();
return result;
}
#endif
default:
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
}
return mp_const_none;
}
static MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_read_depth_obj, 0, py_tof_read_depth);
mp_obj_t py_tof_draw_depth(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum {
ARG_x, ARG_y, ARG_x_scale, ARG_y_scale, ARG_roi, ARG_channel, ARG_alpha,
ARG_color_palette, ARG_alpha_palette, ARG_hint, ARG_scale
};
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_x, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = 0 } },
{ MP_QSTR_y, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = 0 } },
{ MP_QSTR_x_scale, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_y_scale, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_roi, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_rgb_channel, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = -1 } },
{ MP_QSTR_alpha, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = 255 } },
{ MP_QSTR_color_palette, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_INT(COLOR_PALETTE_DEPTH)} },
{ MP_QSTR_alpha_palette, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_hint, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = 0 } },
{ MP_QSTR_scale, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
};
// Parse args.
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
mp_arg_parse_all(n_args - 2, pos_args + 2, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
// Sanity checks
if (tof_sensor == OMV_TOF_NONE) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
}
if (args[ARG_channel].u_int < -1 || args[ARG_channel].u_int > 2) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("RGB channel can be 0, 1, or 2"));
}
if (args[ARG_alpha].u_int < 0 || args[ARG_alpha].u_int > 255) {
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("Alpha ranges between 0 and 255"));
}
image_t src_img = {
.w = tof_transposed ? tof_height : tof_width,
.h = tof_transposed ? tof_width : tof_height,
.pixfmt = PIXFORMAT_GRAYSCALE,
//.data is allocated later.
};
image_t *dst_img = py_helper_arg_to_image(pos_args[0], ARG_IMAGE_MUTABLE);
mp_obj_t *depth_array;
mp_obj_get_array_fixed_n(pos_args[1], src_img.w * src_img.h, &depth_array);
rectangle_t roi = py_helper_arg_to_roi(args[ARG_roi].u_obj, &src_img);
float x_scale = 1.0f;
float y_scale = 1.0f;
py_helper_arg_to_scale(args[ARG_x_scale].u_obj, args[ARG_y_scale].u_obj, &x_scale, &y_scale);
float min = FLT_MAX;
float max = -FLT_MAX;
py_helper_arg_to_minmax(args[ARG_scale].u_obj, &min, &max, depth_array, src_img.w * src_img.h);
const uint16_t *color_palette = py_helper_arg_to_palette(args[ARG_color_palette].u_obj, PIXFORMAT_RGB565);
const uint8_t *alpha_palette = py_helper_arg_to_palette(args[ARG_alpha_palette].u_obj, PIXFORMAT_GRAYSCALE);
fb_alloc_mark();
src_img.data = fb_alloc(src_img.w * src_img.h * sizeof(uint8_t), FB_ALLOC_NO_HINT);
tof_fill_image_float_obj(&src_img, depth_array, min, max);
imlib_draw_image(dst_img, &src_img, args[ARG_x].u_int, args[ARG_y].u_int, x_scale, y_scale, &roi,
args[ARG_channel].u_int, args[ARG_alpha].u_int, color_palette, alpha_palette,
args[ARG_hint].u_int, NULL, NULL, NULL);
fb_alloc_free_till_mark();
return mp_const_none;
}
static MP_DEFINE_CONST_FUN_OBJ_KW(py_tof_draw_depth_obj, 2, py_tof_draw_depth);
mp_obj_t py_tof_snapshot(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
enum {
ARG_hmirror, ARG_vflip, ARG_transpose, ARG_x_scale, ARG_y_scale, ARG_roi, ARG_channel,
ARG_alpha, ARG_color_palette, ARG_alpha_palette, ARG_hint, ARG_scale, ARG_pixformat,
ARG_copy_to_fb, ARG_timeout
};
static const mp_arg_t allowed_args[] = {
{ MP_QSTR_hmirror, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_bool = false } },
{ MP_QSTR_vflip, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_bool = false } },
{ MP_QSTR_transpose, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_bool = false } },
{ MP_QSTR_x_scale, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_y_scale, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_roi, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_rgb_channel, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = -1 } },
{ MP_QSTR_alpha, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = 255 } },
{ MP_QSTR_color_palette, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_INT(COLOR_PALETTE_DEPTH)} },
{ MP_QSTR_alpha_palette, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_hint, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = 0 } },
{ MP_QSTR_scale, MP_ARG_OBJ | MP_ARG_KW_ONLY, {.u_rom_obj = MP_ROM_NONE} },
{ MP_QSTR_pixformat, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = PIXFORMAT_RGB565 } },
{ MP_QSTR_copy_to_fb, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_bool = false } },
{ MP_QSTR_timeout, MP_ARG_INT | MP_ARG_KW_ONLY, {.u_int = -1 } },
};
// Parse args.
mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
mp_arg_parse_all(n_args, pos_args, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
// Sanity checks
if (args[ARG_channel].u_int < -1 || args[ARG_channel].u_int > 2) {
mp_raise_msg(&mp_type_ValueError, MP_ERROR_TEXT("RGB channel can be 0, 1, or 2"));
}
if (args[ARG_alpha].u_int < 0 || args[ARG_alpha].u_int > 255) {
mp_raise_msg(&mp_type_ValueError, MP_ERROR_TEXT("Alpha ranges between 0 and 255"));
}
if ((args[ARG_pixformat].u_int != PIXFORMAT_GRAYSCALE) && (args[ARG_pixformat].u_int != PIXFORMAT_RGB565)) {
mp_raise_msg(&mp_type_ValueError, MP_ERROR_TEXT("Invalid pixformat"));
}
image_t src_img = {
.w = args[ARG_transpose].u_bool ? tof_height : tof_width,
.h = args[ARG_transpose].u_bool ? tof_width : tof_height,
.pixfmt = PIXFORMAT_GRAYSCALE,
//.data is allocated later.
};
rectangle_t roi = py_helper_arg_to_roi(args[ARG_roi].u_obj, &src_img);
float x_scale = 1.0f;
float y_scale = 1.0f;
py_helper_arg_to_scale(args[ARG_x_scale].u_obj, args[ARG_y_scale].u_obj, &x_scale, &y_scale);
image_t dst_img = {
.w = fast_floorf(roi.w * x_scale),
.h = fast_floorf(roi.h * y_scale),
.pixfmt = args[ARG_pixformat].u_int,
};
if (args[ARG_copy_to_fb].u_bool) {
py_helper_set_to_framebuffer(&dst_img);
} else {
image_alloc(&dst_img, image_size(&dst_img));
}
float min = FLT_MAX;
float max = -FLT_MAX;
py_helper_arg_to_minmax(args[ARG_scale].u_obj, &min, &max, NULL, 0);
const uint16_t *color_palette = py_helper_arg_to_palette(args[ARG_color_palette].u_obj, PIXFORMAT_RGB565);
const uint8_t *alpha_palette = py_helper_arg_to_palette(args[ARG_alpha_palette].u_obj, PIXFORMAT_GRAYSCALE);
fb_alloc_mark();
// Allocate source image data.
src_img.data = fb_alloc(src_img.w * src_img.h * sizeof(uint8_t), FB_ALLOC_NO_HINT);
switch (tof_sensor) {
#if OMV_TOF_VL53LX_ENABLE
case OMV_TOF_VL53LX_ID: {
float *frame = fb_alloc(OMV_TOF_VL53LX_WIDTH * OMV_TOF_VL53LX_HEIGHT * sizeof(float),
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) {
fast_get_min_max(frame, OMV_TOF_VL53LX_WIDTH * OMV_TOF_VL53LX_HEIGHT, &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_transpose].u_bool, true);
break;
}
#endif
default:
mp_raise_msg(&mp_type_RuntimeError, MP_ERROR_TEXT("TOF sensor is not initialized"));
}
imlib_draw_image(&dst_img, &src_img, 0, 0, x_scale, y_scale, &roi,
args[ARG_channel].u_int, args[ARG_alpha].u_int, color_palette, alpha_palette,
(args[ARG_hint].u_int & (~IMAGE_HINT_CENTER)) | IMAGE_HINT_BLACK_BACKGROUND, NULL, NULL, NULL);
fb_alloc_free_till_mark();
if (args[ARG_copy_to_fb].u_bool) {
framebuffer_update_jpeg_buffer(&dst_img);
}
return py_image_from_struct(&dst_img);
}
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[] = {
{ MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_tof) },
#if OMV_TOF_VL53LX_ENABLE
{ MP_ROM_QSTR(MP_QSTR_TOF_VL53LX), MP_ROM_INT(OMV_TOF_VL53LX_ID) },
#endif
{ 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_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_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_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_draw_depth), MP_ROM_PTR(&py_tof_draw_depth_obj) },
{ MP_ROM_QSTR(MP_QSTR_snapshot), MP_ROM_PTR(&py_tof_snapshot_obj) }
};
static MP_DEFINE_CONST_DICT(globals_dict, globals_dict_table);
const mp_obj_module_t tof_module = {
.base = { &mp_type_module },
.globals = (mp_obj_t) &globals_dict,
};
MP_REGISTER_MODULE(MP_QSTR_tof, tof_module);
#endif