VL53L5CX: Bringup Motion and add example.

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) {

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;
     };
 }

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();

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(

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)

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
 )
 

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) },

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);
 }

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);