44#include " Wire.h"
55#include < vector>
66#include < vl53l4cd_class.h> // from stm32duino
7+ #include < vl53l4ed_class.h> // from stm32duino
78#include " Arduino_LSM6DSOX.h"
89#include < Arduino_LPS22HB.h>
910#include < Arduino_HS300x.h>
@@ -396,6 +397,52 @@ class ModulinoLight : public Module {
396397
397398};
398399
400+ class _distance_api {
401+ public:
402+ _distance_api (VL53L4CD* sensor) : sensor(sensor) {
403+ isVL53L4CD = true ;
404+ };
405+ _distance_api (VL53L4ED* sensor) : sensor(sensor) {};
406+ uint8_t setRangeTiming (uint32_t timing_budget_ms, uint32_t inter_measurement_ms) {
407+ if (isVL53L4CD) {
408+ return ((VL53L4CD*)sensor)->VL53L4CD_SetRangeTiming (timing_budget_ms, inter_measurement_ms);
409+ } else {
410+ return ((VL53L4ED*)sensor)->VL53L4ED_SetRangeTiming (timing_budget_ms, inter_measurement_ms);
411+ }
412+ }
413+ uint8_t startRanging () {
414+ if (isVL53L4CD) {
415+ return ((VL53L4CD*)sensor)->VL53L4CD_StartRanging ();
416+ } else {
417+ return ((VL53L4ED*)sensor)->VL53L4ED_StartRanging ();
418+ }
419+ }
420+ uint8_t checkForDataReady (uint8_t * p_is_data_ready) {
421+ if (isVL53L4CD) {
422+ return ((VL53L4CD*)sensor)->VL53L4CD_CheckForDataReady (p_is_data_ready);
423+ } else {
424+ return ((VL53L4ED*)sensor)->VL53L4ED_CheckForDataReady (p_is_data_ready);
425+ }
426+ }
427+ uint8_t clearInterrupt () {
428+ if (isVL53L4CD) {
429+ return ((VL53L4CD*)sensor)->VL53L4CD_ClearInterrupt ();
430+ } else {
431+ return ((VL53L4ED*)sensor)->VL53L4ED_ClearInterrupt ();
432+ }
433+ }
434+ uint8_t getResult (void * result) {
435+ if (isVL53L4CD) {
436+ return ((VL53L4CD*)sensor)->VL53L4CD_GetResult ((VL53L4CD_Result_t*)result);
437+ } else {
438+ return ((VL53L4ED*)sensor)->VL53L4ED_GetResult ((VL53L4ED_ResultsData_t*)result);
439+ }
440+ }
441+ private:
442+ void * sensor;
443+ bool isVL53L4CD = false ;
444+ };
445+ 399446class ModulinoDistance : public Module {
400447public:
401448 bool begin () {
@@ -406,29 +453,40 @@ class ModulinoDistance : public Module {
406453 }
407454 tof_sensor = new VL53L4CD ((TwoWire*)getWire (), -1 );
408455 auto ret = tof_sensor->InitSensor ();
409- __increaseI2CPriority ();
410- if (ret == VL53L4CD_ERROR_NONE) {
411- tof_sensor->VL53L4CD_SetRangeTiming (20 , 0 );
412- tof_sensor->VL53L4CD_StartRanging ();
413- return true ;
414- } else {
456+ if (ret != VL53L4CD_ERROR_NONE) {
457+ delete tof_sensor;
415458 tof_sensor = nullptr ;
416- return false ;
459+ tof_sensor_alt = new VL53L4ED ((TwoWire*)getWire (), -1 );
460+ ret = tof_sensor_alt->InitSensor ();
461+ if (ret == VL53L4ED_ERROR_NONE) {
462+ api = new _distance_api (tof_sensor_alt);
463+ } else {
464+ delete tof_sensor_alt;
465+ tof_sensor_alt = nullptr ;
466+ return false ;
467+ }
468+ } else {
469+ api = new _distance_api (tof_sensor);
417470 }
471+ 472+ __increaseI2CPriority ();
473+ api->setRangeTiming (20 , 0 );
474+ api->startRanging ();
475+ return true ;
418476 }
419477 operator bool () {
420- return (tof_sensor != nullptr );
478+ return (api != nullptr );
421479 }
422480 bool available () {
423- if (tof_sensor == nullptr ) {
481+ if (api == nullptr ) {
424482 return false ;
425483 }
426484 float ret = internal;
427485 uint8_t NewDataReady = 0 ;
428- tof_sensor-> VL53L4CD_CheckForDataReady (&NewDataReady);
486+ api-> checkForDataReady (&NewDataReady);
429487 if (NewDataReady) {
430- tof_sensor-> VL53L4CD_ClearInterrupt ();
431- tof_sensor-> VL53L4CD_GetResult (&results);
488+ api-> clearInterrupt ();
489+ api-> getResult (&results);
432490 }
433491 if (results.range_status == 0 ) {
434492 internal = results.distance_mm ;
@@ -442,6 +500,9 @@ class ModulinoDistance : public Module {
442500 }
443501private:
444502 VL53L4CD* tof_sensor = nullptr ;
503+ VL53L4ED* tof_sensor_alt = nullptr ;
445504 VL53L4CD_Result_t results;
505+ // VL53L4ED_ResultsData_t results;
446506 float internal = NAN;
507+ _distance_api* api = nullptr ;
447508};
0 commit comments