From 3afead416a7df9ba65d03ac658a8b038544f5649 Mon Sep 17 00:00:00 2001 From: Phil Howard Date: Thu, 28 Apr 2022 14:38:34 +0100 Subject: [PATCH] VL53L5CX: Bringup Motion and add example. --- drivers/vl53l5cx/vl53l5cx.cpp | 18 +++++++++- drivers/vl53l5cx/vl53l5cx.hpp | 10 ++++++ examples/breakout_vl53l5cx/vl53l5cx_demo.cpp | 5 ++- .../breakout_vl53l5cx/vl53l5cx_demo.py | 2 +- .../breakout_vl53l5cx/vl53l5cx_motion.py | 35 +++++++++++++++++++ .../breakout_vl53l5cx/micropython.cmake | 1 + .../modules/breakout_vl53l5cx/vl53l5cx.c | 6 ++++ .../modules/breakout_vl53l5cx/vl53l5cx.cpp | 30 ++++++++++++---- .../modules/breakout_vl53l5cx/vl53l5cx.h | 3 ++ 9 files changed, 101 insertions(+), 9 deletions(-) create mode 100644 micropython/examples/breakout_vl53l5cx/vl53l5cx_motion.py diff --git a/drivers/vl53l5cx/vl53l5cx.cpp b/drivers/vl53l5cx/vl53l5cx.cpp index b550d71b..d962f21b 100644 --- a/drivers/vl53l5cx/vl53l5cx.cpp +++ b/drivers/vl53l5cx/vl53l5cx.cpp @@ -2,9 +2,17 @@ namespace pimoroni { bool VL53L5CX::init() { + if(!is_alive()) { + return false; + } uint8_t status = vl53l5cx_init(configuration); return status == VL53L5CX_STATUS_OK; } + bool VL53L5CX::is_alive() { + uint8_t is_alive = 0; + uint8_t status = vl53l5cx_is_alive(configuration, &is_alive); + return is_alive == 1 && status == VL53L5CX_STATUS_OK; + } bool VL53L5CX::start_ranging() { uint8_t status = vl53l5cx_start_ranging(configuration); return status == VL53L5CX_STATUS_OK; @@ -13,9 +21,17 @@ namespace pimoroni { uint8_t status = vl53l5cx_stop_ranging(configuration); return status == VL53L5CX_STATUS_OK; } + bool VL53L5CX::enable_motion_indicator(Resolution resolution) { + uint8_t status = vl53l5cx_motion_indicator_init(configuration, motion_configuration, resolution); + return status == VL53L5CX_STATUS_OK; + } + bool VL53L5CX::set_motion_distance(uint16_t distance_min, uint16_t distance_max) { + uint8_t status = vl53l5cx_motion_indicator_set_distance_motion(configuration, motion_configuration, distance_min, distance_max); + return status == VL53L5CX_STATUS_OK; + } bool VL53L5CX::set_i2c_address(uint8_t i2c_address) { /* Must be a 7-bit i2c address */ - uint8_t status = vl53l5cx_set_i2c_address(configuration, i2c_address << 1); + uint8_t status = vl53l5cx_set_i2c_address(configuration, i2c_address); return status == VL53L5CX_STATUS_OK; } bool VL53L5CX::set_ranging_mode(RangingMode ranging_mode) { diff --git a/drivers/vl53l5cx/vl53l5cx.hpp b/drivers/vl53l5cx/vl53l5cx.hpp index af58917b..9198c58f 100644 --- a/drivers/vl53l5cx/vl53l5cx.hpp +++ b/drivers/vl53l5cx/vl53l5cx.hpp @@ -2,10 +2,12 @@ extern "C" { #include "drivers/vl53l5cx/src/VL53L5CX_ULD_API/inc/vl53l5cx_api.h" +#include "drivers/vl53l5cx/src/VL53L5CX_ULD_API/inc/vl53l5cx_plugin_motion_indicator.h" } #include "common/pimoroni_i2c.hpp" #include "src/VL53L5CX_ULD_API/inc/vl53l5cx_api.h" +#include "src/VL53L5CX_ULD_API/inc/vl53l5cx_plugin_motion_indicator.h" namespace pimoroni { class VL53L5CX { @@ -38,13 +40,20 @@ namespace pimoroni { .i2c = i2c->get_i2c() }, }; + motion_configuration = new VL53L5CX_Motion_Configuration{}; } ~VL53L5CX() { delete configuration; + delete motion_configuration; } bool init(); + bool is_alive(); bool start_ranging(); bool stop_ranging(); + + bool enable_motion_indicator(Resolution resolution); + bool set_motion_distance(uint16_t distance_min, uint16_t distance_max); + bool set_i2c_address(uint8_t i2c_address); bool set_ranging_mode(RangingMode ranging_mode); bool set_ranging_frequency_hz(uint8_t ranging_frequency_hz); @@ -64,6 +73,7 @@ namespace pimoroni { } private: VL53L5CX_Configuration *configuration; + VL53L5CX_Motion_Configuration *motion_configuration; Resolution resolution = RESOLUTION_8X8; }; } \ No newline at end of file diff --git a/examples/breakout_vl53l5cx/vl53l5cx_demo.cpp b/examples/breakout_vl53l5cx/vl53l5cx_demo.cpp index d6e6f5ff..c0eb829d 100644 --- a/examples/breakout_vl53l5cx/vl53l5cx_demo.cpp +++ b/examples/breakout_vl53l5cx/vl53l5cx_demo.cpp @@ -13,7 +13,10 @@ VL53L5CX vl53l5cx(&i2c); int main() { stdio_init_all(); - vl53l5cx.init(); + bool result = vl53l5cx.init(); + if(!result) { + printf("Error initializing...\n"); + } vl53l5cx.set_ranging_mode(VL53L5CX::RANGING_MODE_AUTONOMOUS); vl53l5cx.set_resolution(VL53L5CX::RESOLUTION_4X4); vl53l5cx.start_ranging(); diff --git a/micropython/examples/breakout_vl53l5cx/vl53l5cx_demo.py b/micropython/examples/breakout_vl53l5cx/vl53l5cx_demo.py index d38296ba..ff0f1edd 100644 --- a/micropython/examples/breakout_vl53l5cx/vl53l5cx_demo.py +++ b/micropython/examples/breakout_vl53l5cx/vl53l5cx_demo.py @@ -22,7 +22,7 @@ sensor.start_ranging() while True: if sensor.data_ready(): # "data" is a namedtuple (attrtuple technically) - # it includes average readings as "distance_avg" and "reflectance_avg"a + # it includes average readings as "distance_avg" and "reflectance_avg" # plus a full 4x4 or 8x8 set of readings (as a 1d tuple) for both values. data = sensor.get_data() print("{}mm {}% (avg: {}mm {}%)".format( diff --git a/micropython/examples/breakout_vl53l5cx/vl53l5cx_motion.py b/micropython/examples/breakout_vl53l5cx/vl53l5cx_motion.py new file mode 100644 index 00000000..10ffec38 --- /dev/null +++ b/micropython/examples/breakout_vl53l5cx/vl53l5cx_motion.py @@ -0,0 +1,35 @@ +import pimoroni_i2c +import breakout_vl53l5cx +import time +from ulab import numpy + +PINS_BREAKOUT_GARDEN = {"sda": 4, "scl": 5} +PINS_PICO_EXPLORER = {"sda": 20, "scl": 21} + +# Sensor startup time is proportional to i2c baudrate +# HOWEVER many sensors may not run at > 400KHz (400000) +i2c = pimoroni_i2c.PimoroniI2C(**PINS_BREAKOUT_GARDEN, baudrate=2_000_000) + +print("Starting up sensor...") +t_sta = time.ticks_ms() +sensor = breakout_vl53l5cx.VL53L5CX(i2c) +t_end = time.ticks_ms() +print("Done in {}ms...".format(t_end - t_sta)) + +# Make sure to set resolution and other settings *before* you start ranging +sensor.set_resolution(breakout_vl53l5cx.RESOLUTION_4X4) + +sensor.enable_motion_indicator(breakout_vl53l5cx.RESOLUTION_4X4) +sensor.set_motion_distance(400, 1400) + +sensor.start_ranging() + +while True: + if sensor.data_ready(): + # "data" is a namedtuple (attrtuple technically) + # it includes average readings as "distance_avg" and "reflectance_avg" + # plus a full 4x4 or 8x8 set of readings (as a 1d tuple) for both values. + # Motion data is available in "motion_detection.motion" + data = sensor.get_data() + motion = numpy.array(data.motion_indicator.motion[0:16], dtype=numpy.int16).reshape((4, 4)) + print(motion) diff --git a/micropython/modules/breakout_vl53l5cx/micropython.cmake b/micropython/modules/breakout_vl53l5cx/micropython.cmake index 08d106df..71880809 100644 --- a/micropython/modules/breakout_vl53l5cx/micropython.cmake +++ b/micropython/modules/breakout_vl53l5cx/micropython.cmake @@ -6,6 +6,7 @@ target_sources(usermod_vl53l5cx INTERFACE ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/vl53l5cx/vl53l5cx.cpp ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/vl53l5cx/platform.c ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/vl53l5cx/src/VL53L5CX_ULD_API/src/vl53l5cx_api.c + ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/vl53l5cx/src/VL53L5CX_ULD_API/src/vl53l5cx_plugin_motion_indicator.c ${CMAKE_CURRENT_LIST_DIR}/../../../drivers/vl53l5cx/src/VL53L5CX_ULD_API/src/vl53l5cx_plugin_detection_thresholds.c ) diff --git a/micropython/modules/breakout_vl53l5cx/vl53l5cx.c b/micropython/modules/breakout_vl53l5cx/vl53l5cx.c index 8b614845..04be5574 100644 --- a/micropython/modules/breakout_vl53l5cx/vl53l5cx.c +++ b/micropython/modules/breakout_vl53l5cx/vl53l5cx.c @@ -6,6 +6,9 @@ MP_DEFINE_CONST_FUN_OBJ_1(VL53L5CX___del___obj, VL53L5CX___del__); MP_DEFINE_CONST_FUN_OBJ_1(VL53L5CX_start_ranging_obj, VL53L5CX_start_ranging); MP_DEFINE_CONST_FUN_OBJ_1(VL53L5CX_stop_ranging_obj, VL53L5CX_stop_ranging); +MP_DEFINE_CONST_FUN_OBJ_2(VL53L5CX_enable_motion_indicator_obj, VL53L5CX_enable_motion_indicator); +MP_DEFINE_CONST_FUN_OBJ_3(VL53L5CX_set_motion_distance_obj, VL53L5CX_set_motion_distance); + MP_DEFINE_CONST_FUN_OBJ_2(VL53L5CX_set_i2c_address_obj, VL53L5CX_set_i2c_address); MP_DEFINE_CONST_FUN_OBJ_2(VL53L5CX_set_ranging_mode_obj, VL53L5CX_set_ranging_mode); MP_DEFINE_CONST_FUN_OBJ_2(VL53L5CX_set_ranging_frequency_hz_obj, VL53L5CX_set_ranging_frequency_hz); @@ -26,6 +29,9 @@ STATIC const mp_rom_map_elem_t VL53L5CX_locals_dict_table[] = { { MP_ROM_QSTR(MP_QSTR_start_ranging), MP_ROM_PTR(&VL53L5CX_start_ranging_obj) }, { MP_ROM_QSTR(MP_QSTR_stop_ranging), MP_ROM_PTR(&VL53L5CX_stop_ranging_obj) }, + { MP_ROM_QSTR(MP_QSTR_enable_motion_indicator), MP_ROM_PTR(&VL53L5CX_enable_motion_indicator_obj) }, + { MP_ROM_QSTR(MP_QSTR_set_motion_distance), MP_ROM_PTR(&VL53L5CX_set_motion_distance_obj) }, + { MP_ROM_QSTR(MP_QSTR_set_i2c_address), MP_ROM_PTR(&VL53L5CX_set_i2c_address_obj) }, { MP_ROM_QSTR(MP_QSTR_set_ranging_mode), MP_ROM_PTR(&VL53L5CX_set_ranging_mode_obj) }, { MP_ROM_QSTR(MP_QSTR_set_ranging_frequency_hz), MP_ROM_PTR(&VL53L5CX_set_ranging_frequency_hz_obj) }, diff --git a/micropython/modules/breakout_vl53l5cx/vl53l5cx.cpp b/micropython/modules/breakout_vl53l5cx/vl53l5cx.cpp index 89f85703..662a982f 100644 --- a/micropython/modules/breakout_vl53l5cx/vl53l5cx.cpp +++ b/micropython/modules/breakout_vl53l5cx/vl53l5cx.cpp @@ -110,6 +110,24 @@ mp_obj_t VL53L5CX_stop_ranging(mp_obj_t self_in) { return mp_const_none; } +mp_obj_t VL53L5CX_enable_motion_indicator(mp_obj_t self_in, mp_obj_t value) { + _VL53L5CX_obj_t *self = MP_OBJ_TO_PTR2(self_in, _VL53L5CX_obj_t); + bool status = self->breakout->enable_motion_indicator((pimoroni::VL53L5CX::Resolution)mp_obj_get_int(value)); + if(!status) { + mp_raise_msg(&mp_type_RuntimeError, "VL53L5CX: enable_motion_indicator error"); + } + return mp_const_none; +} + +mp_obj_t VL53L5CX_set_motion_distance(mp_obj_t self_in, mp_obj_t distance_min, mp_obj_t distance_max) { + _VL53L5CX_obj_t *self = MP_OBJ_TO_PTR2(self_in, _VL53L5CX_obj_t); + bool status = self->breakout->set_motion_distance(mp_obj_get_int(distance_min), mp_obj_get_int(distance_max)); + if(!status) { + mp_raise_msg(&mp_type_RuntimeError, "VL53L5CX: set_motion_distance error"); + } + return mp_const_none; +} + mp_obj_t VL53L5CX_set_i2c_address(mp_obj_t self_in, mp_obj_t value) { _VL53L5CX_obj_t *self = MP_OBJ_TO_PTR2(self_in, _VL53L5CX_obj_t); bool status = self->breakout->set_i2c_address(mp_obj_get_int(value)); @@ -220,30 +238,30 @@ mp_obj_t VL53L5CX_get_data(mp_obj_t self_in) { average_distance /= tuple_size; average_reflectance /= tuple_size; - // TODO motion data is all zeros, why? - - /*mp_obj_t tuple_motion_data[32]; + mp_obj_t tuple_motion_data[32]; for(int i = 0u; i < 32; i++) { tuple_motion_data[i] = mp_obj_new_int(results.motion_indicator.motion[i]); } + STATIC const qstr tuple_motion_fields[] = {MP_QSTR_global_indicator_1, MP_QSTR_global_indicator_2, MP_QSTR_motion}; + mp_obj_t tuple_motion[] = { mp_obj_new_int(results.motion_indicator.global_indicator_1), mp_obj_new_int(results.motion_indicator.global_indicator_2), mp_obj_new_tuple(sizeof(tuple_motion_data) / sizeof(mp_obj_t), tuple_motion_data) - };*/ + }; mp_obj_t tuple[] = { mp_obj_new_int(average_distance), // Average distance mp_obj_new_int(average_reflectance), // Average reflectance - //mp_obj_new_tuple(sizeof(tuple_motion) / sizeof(mp_obj_t), tuple_motion), // Motion data + mp_obj_new_attrtuple(tuple_motion_fields, sizeof(tuple_motion) / sizeof(mp_obj_t), tuple_motion), // Motion data mp_obj_new_int(tuple_size), // Number of results mp_obj_new_tuple(tuple_size, tuple_distance_mm), // Full distance results mp_obj_new_tuple(tuple_size, tuple_reflectance) // Full reflectange results }; - STATIC const qstr tuple_fields[] = {MP_QSTR_distance_avg, MP_QSTR_reflectance_avg, MP_QSTR_results, MP_QSTR_distance, MP_QSTR_reflectance}; + STATIC const qstr tuple_fields[] = {MP_QSTR_distance_avg, MP_QSTR_reflectance_avg, MP_QSTR_motion_indicator, MP_QSTR_results, MP_QSTR_distance, MP_QSTR_reflectance}; return mp_obj_new_attrtuple(tuple_fields, sizeof(tuple) / sizeof(mp_obj_t), tuple); } diff --git a/micropython/modules/breakout_vl53l5cx/vl53l5cx.h b/micropython/modules/breakout_vl53l5cx/vl53l5cx.h index 50470a74..74c151eb 100644 --- a/micropython/modules/breakout_vl53l5cx/vl53l5cx.h +++ b/micropython/modules/breakout_vl53l5cx/vl53l5cx.h @@ -11,6 +11,9 @@ extern mp_obj_t VL53L5CX_make_new(const mp_obj_type_t *type, size_t n_args, size extern mp_obj_t VL53L5CX_start_ranging(mp_obj_t self_in); extern mp_obj_t VL53L5CX_stop_ranging(mp_obj_t self_in); +extern mp_obj_t VL53L5CX_enable_motion_indicator(mp_obj_t self_in, mp_obj_t value); +extern mp_obj_t VL53L5CX_set_motion_distance(mp_obj_t self_in, mp_obj_t distance_min, mp_obj_t distance_max); + extern mp_obj_t VL53L5CX_set_i2c_address(mp_obj_t self_in, mp_obj_t value); extern mp_obj_t VL53L5CX_set_ranging_mode(mp_obj_t self_in, mp_obj_t value); extern mp_obj_t VL53L5CX_set_ranging_frequency_hz(mp_obj_t self_in, mp_obj_t value);