[ ToF Integration ] - reduced to 20Hz

Firmware/ESP32/Arduino_platform/include/avr_driver_common.h

@@ -116,10 +116,10 @@ typedef enum motor_command_direction{
 // tof_sensor_config
 // XSHUT is active low, pulling low shutsdown the sensor 
 typedef enum tof_sensor_config{
-    TOF_SENSOR_CONFIG_DISABLE_ALL,  //pull XSHUT low, sensor shutdown (active low )
-    TOF_SENSOR_CONFIG_ENABLE_1,
-    TOF_SENSOR_CONFIG_ENABLE_2,
-    TOF_SENSOR_CONFIG_ENABLE_3,
+    TOF_SENSOR_CONFIG_ENABLE_ALL  = 0U,  //pull XSHUT high, sensor shutdown (active high)
+    TOF_SENSOR_CONFIG_ENABLE_TWO  = 1U,
+    TOF_SENSOR_CONFIG_ENABLE_ONE  = 2U,
+    TOF_SENSOR_CONFIG_DISABLE_ALL = 3U,
     TOF_SENSOR_CONFIG_COUNT,
     TOF_SENSOR_CONFIG_UNDEFINED
 } tof_sensor_config_E; 

Firmware/ESP32/Arduino_platform/include/common.h

@@ -72,13 +72,17 @@ extern "C"{
 #   define MOCK                                   ( ENABLE)
 
 /*****   FEATURE ENABLES  ****/
-#   define FEATURE_SLAM                           (DISABLE) // APP SLAM
-#   define FEATURE_LIDAR                          (DISABLE)
+#   define FEATURE_DEMO_TOF_OBSTACLE              ( ENABLE) // DEV avr driver: motor, mist, encoder feedback
+#   define FEATURE_DEV_DRIVER                     ( ENABLE) // DEV avr driver: motor, mist, encoder feedback
+#   define FEATURE_SLAM                           ( ENABLE) // APP SLAM
+#   define FEATURE_LIDAR                          ( ENABLE)
 #   define FEATURE_LIDAR_CALIBRATION_MODE         (   TODO) // TODO: implement calibration strategy
 #   define FEATURE_SUPER_USE_PROFILED_MOTIONS     (   TODO) // TODO: implement calibration strategy
 #   define FEATURE_SUPER_USE_HARDCODE_CHORE       ( ENABLE)
+#   define FEATURE_SUPER_CMD_DEV_DRIVER           ( ENABLE) // Super command on actuators
 #   define FEATURE_PERIPHERALS                    ( ENABLE)
-#   define FEATURE_UV                             ( DISABLE)
+#   define FEATURE_FAST_MODE                      (DISABLE) // Wacky shit | Attempt to use 50Hz, currently it is safe to use 20Hz, to increase, we need to find a way to increase ToF to 400kHz.
+#   define FEATURE_UV                             ((DISABLE) && (! FEATURE_FAST_MODE))
 #   define FEATURE_IMU                            (DISABLE)
 #   define FEATURE_SENSOR_AVR                     ( ENABLE)
 #   define FEATURE_AVR_DRIVER_ALL                 ( ENABLE) //
@@ -94,19 +98,23 @@ extern "C"{
 /*****   DEBUG PRINT FLAGS  ****/
 #   define DEBUG_FPRINT                           ( ENABLE)
 #   ifdef DEBUG_FPRINT
-#       define DEBUG_FPRINT_FEATURE_LIDAR         (DISABLE) // Live feed of sensor readings
-#       define DEBUG_FPRINT_APP_SUPERVISOR        (DISABLE) // Live feed of collision status
-#       define DEBUG_FPRINT_FEATURE_MAP           (DISABLE) // Live feed of global map
-#       define DEBUG_FPRINT_FEATURE_MAP_CENTERED  (   TRUE) // Map Feeding Mode: true->memory_map, false->center_map
-#       define DEBUG_FPRINT_FEATURE_OBSTACLES     (DISABLE) // Live feed of obstacle detection
-#       define DEBUG_FPRINT_FEATURE_CHOREOGRAPHY  (DISABLE) // Live feed of choreography
+#       define DEBUG_FPRINT_FEATURE_LIDAR               ( ENABLE) // Live feed of tof sensor readings
+#       define DEBUG_FPRINT_APP_SUPER_STATE             ( ENABLE) // Live feed of supervisor state
+#       define DEBUG_FPRINT_APP_SUPER_AVR_SENSOR        ( ENABLE) // Live feed of collision status
+#       define DEBUG_FPRINT_APP_SUPER_CHOREOGRAPHY      ( ENABLE) // Live feed of choreography status
+#       define DEBUG_FPRINT_FEATURE_MAP                 (DISABLE) // Live feed of global map
+#       define DEBUG_FPRINT_FEATURE_MAP_CENTERED        (   TRUE) // Map Feeding Mode: true->memory_map, false->center_map
+#       define DEBUG_FPRINT_FEATURE_OBSTACLES           (DISABLE) // Live feed of obstacle detection
+#       define DEBUG_FPRINT_FEATURE_CHOREOGRAPHY        (DISABLE) // Live feed of choreography
 #   else
-#       define DEBUG_FPRINT_FEATURE_LIDAR         (DISABLE)
-#       define DEBUG_FPRINT_APP_SUPERVISOR        (DISABLE)
-#       define DEBUG_FPRINT_FEATURE_MAP           (DISABLE)
-#       define DEBUG_FPRINT_FEATURE_MAP_CENTERED  (DISABLE)
-#       define DEBUG_FPRINT_FEATURE_OBSTACLES     (DISABLE)
-#       define DEBUG_FPRINT_FEATURE_CHOREOGRAPHY  (DISABLE)
+#       define DEBUG_FPRINT_FEATURE_LIDAR               (DISABLE)
+#       define DEBUG_FPRINT_APP_SUPER_STATE             (DISABLE)
+#       define DEBUG_FPRINT_APP_SUPER_AVR_SENSOR        (DISABLE)
+#       define DEBUG_FPRINT_APP_SUPER_CHOREOGRAPHY      (DISABLE)
+#       define DEBUG_FPRINT_FEATURE_MAP                 (DISABLE)
+#       define DEBUG_FPRINT_FEATURE_MAP_CENTERED        (DISABLE)
+#       define DEBUG_FPRINT_FEATURE_OBSTACLES           (DISABLE)
+#       define DEBUG_FPRINT_FEATURE_CHOREOGRAPHY        (DISABLE)
 #   endif
 
 /***********************************

Firmware/ESP32/Arduino_platform/lib/DEV/dev_ToF_Lidar.cpp

@@ -18,6 +18,7 @@
 // TableUV Lib
 #include "../IO/io_ping_map.h"
 #include "../../include/common.h"
+#include "dev_avr_driver.h"
 
 // Arduino Lib
 #include <SparkFun_VL53L1X.h>
@@ -30,14 +31,15 @@
 #define TOF_WIDTH_OF_SPADS_PER_ZONE         (5U) // MIN: 4 pix
 #define TOF_HEIGHT_OF_SPADS_PER_ZONE        (5U) // MIN: 4 pix
 #define TOF_MAX_DIST_MM                     (1300U) // 1.3 [m] in short range mode
-#define TOF_INTERMEDIATE_SETTING_DELAY_MS   (20U)
+#define TOF_INTERMEDIATE_SETTING_DELAY_MS   (10U)
 #define MP_MUTEX_BLOCK_TIME_MS              ((1U)/portTICK_PERIOD_MS)
-#define TOF_SENSOR_COUNT                    (1U) // (DEV_TOF_LIDAR_COUNT)
+#define TOF_SENSOR_COUNT                    (DEV_TOF_LIDAR_COUNT)
 
 typedef struct{
     TwoWire             I2C;
     SFEVL53L1X          tofs[DEV_TOF_LIDAR_COUNT];
     const uint8_t       address[DEV_TOF_LIDAR_COUNT];
+    const tof_sensor_config_E avr_config[DEV_TOF_LIDAR_COUNT];
     const uint8_t       firing_sequence[DEV_TOF_FIRING_KEYFRAME_COUNT];
     const uint8_t       firing_sequence_label[DEV_TOF_LIDAR_COUNT][DEV_TOF_FIRING_KEYFRAME_COUNT];
     uint8_t             prev_firingframe[DEV_TOF_LIDAR_COUNT];
@@ -57,14 +59,19 @@ void dev_ToF_reset_all_sensors(void);
 static dev_tof_lidar_data_S lidar_data = {
     .I2C = TwoWire(1),
     .tofs = {
-        SFEVL53L1X(lidar_data.I2C, 21, TOF_INT_1),//TOF_SHUT, TOF_INT_1),
-        SFEVL53L1X(lidar_data.I2C, 22, TOF_INT_2),//TOF_SHUT, TOF_INT_2),
-        SFEVL53L1X(lidar_data.I2C, 23, TOF_INT_3),//TOF_SHUT, TOF_INT_3)
+        /* DEV_TOF_LIDAR_C */ SFEVL53L1X(lidar_data.I2C, -1, TOF_INT_2),//TOF_SHUT, TOF_INT_1),
+        /* DEV_TOF_LIDAR_L */ SFEVL53L1X(lidar_data.I2C, -1, TOF_INT_1),//TOF_SHUT, TOF_INT_2),
+        /* DEV_TOF_LIDAR_R */ SFEVL53L1X(lidar_data.I2C, -1, TOF_INT_3),//TOF_SHUT, TOF_INT_3)
     },
     .address = { 
-        0x62, 
-        0x64, 
-        0x66 // Arbitrary Address (non-default)
+        [DEV_TOF_LIDAR_C] = 0x62, 
+        [DEV_TOF_LIDAR_L] = 0x64, 
+        [DEV_TOF_LIDAR_R] = 0x66, // Arbitrary Address (non-default)
+    },
+    .avr_config = { // MUST BE IN THIS ORDER:
+        [DEV_TOF_LIDAR_C] = TOF_SENSOR_CONFIG_ENABLE_ONE, 
+        [DEV_TOF_LIDAR_L] = TOF_SENSOR_CONFIG_ENABLE_TWO, 
+        [DEV_TOF_LIDAR_R] = TOF_SENSOR_CONFIG_ENABLE_ALL,
     },
 /**Table of Optical Centers**
 *
@@ -95,14 +102,14 @@ static dev_tof_lidar_data_S lidar_data = {
         //Firing Order:  Left <-    [2] - [4] - [1] - [5] - [3]    -> Right
     },
     .firing_sequence_label = {
-        DEV_TOF_FIRING_GEOMETRICAL_3, DEV_TOF_FIRING_GEOMETRICAL_1, DEV_TOF_FIRING_GEOMETRICAL_5, DEV_TOF_FIRING_GEOMETRICAL_2, DEV_TOF_FIRING_GEOMETRICAL_4,
-        DEV_TOF_FIRING_GEOMETRICAL_8, DEV_TOF_FIRING_GEOMETRICAL_6, DEV_TOF_FIRING_GEOMETRICAL_10, DEV_TOF_FIRING_GEOMETRICAL_7, DEV_TOF_FIRING_GEOMETRICAL_9,
-        DEV_TOF_FIRING_GEOMETRICAL_13, DEV_TOF_FIRING_GEOMETRICAL_11, DEV_TOF_FIRING_GEOMETRICAL_15, DEV_TOF_FIRING_GEOMETRICAL_12, DEV_TOF_FIRING_GEOMETRICAL_14,
+        /* DEV_TOF_LIDAR_C */ DEV_TOF_FIRING_GEOMETRICAL_8, DEV_TOF_FIRING_GEOMETRICAL_6, DEV_TOF_FIRING_GEOMETRICAL_10, DEV_TOF_FIRING_GEOMETRICAL_7, DEV_TOF_FIRING_GEOMETRICAL_9,
+        /* DEV_TOF_LIDAR_L */ DEV_TOF_FIRING_GEOMETRICAL_13, DEV_TOF_FIRING_GEOMETRICAL_11, DEV_TOF_FIRING_GEOMETRICAL_15, DEV_TOF_FIRING_GEOMETRICAL_12, DEV_TOF_FIRING_GEOMETRICAL_14,
+        /* DEV_TOF_LIDAR_R */ DEV_TOF_FIRING_GEOMETRICAL_3, DEV_TOF_FIRING_GEOMETRICAL_1, DEV_TOF_FIRING_GEOMETRICAL_5, DEV_TOF_FIRING_GEOMETRICAL_2, DEV_TOF_FIRING_GEOMETRICAL_4,
     },
     .prev_firingframe = {
-        DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
-        DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
-        DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
+        /* DEV_TOF_LIDAR_C */ DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
+        /* DEV_TOF_LIDAR_L */ DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
+        /* DEV_TOF_LIDAR_R */ DEV_TOF_FIRING_KEYFRAME_UNKNOWN,
     },
     .mp_mutex = xSemaphoreCreateBinary(),
     .mp_data ={
@@ -141,40 +148,43 @@ void dev_ToF_reset_all_sensors(void)
 	// int16_t offset;
 
     // start ToFs
-    for (int sensor_id = DEV_TOF_LIDAR_R; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
+    for (int sensor_id = 0U; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
     {
         sensor = & (lidar_data.tofs[sensor_id]);
-        if (sensor->begin() == true)
+        const int8_t begin_status = sensor->begin();
+        if (begin_status == VL53L1X_ERROR_NONE)
         {
-            PRINTF("%d Sensor online!\n", sensor_id);
+            PRINTF("[ DEV:ToF ] Sensor[%d] online! [Code:%d]\n", sensor_id, begin_status);
         }
         else
         {
-            PRINTF("%d Sensor offline!\n", sensor_id);
+            PRINTF("[ DEV:ToF ] Sensor[%d] offline! [Code:%d]\n", sensor_id, begin_status);
         }
-
-        // Take down all sensors
-        sensor->sensorOff();
     }
-
+    // Take down all sensors : DEFAULT: off on AVR
+    dev_avr_driver_set_req_Tof_config(TOF_SENSOR_CONFIG_DISABLE_ALL);
+    dev_driver_avr_update20ms(); // Force update
     delay(TOF_INTERMEDIATE_SETTING_DELAY_MS);
 
     // read sensor initial values & set address & activate sensor
-    for (int sensor_id = DEV_TOF_LIDAR_R; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
+    for (int sensor_id = 0U; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
     {
         sensor = & (lidar_data.tofs[sensor_id]);
 
-        // turn on sensor
-        sensor->sensorOn();
-        int boot = sensor->checkBootState();
+        // turn on sensor | WARNING: Do not call: sensor->sensorOn();
+        dev_avr_driver_set_req_Tof_config(lidar_data.avr_config[sensor_id]);
+        dev_driver_avr_update20ms(); // Force update
+        delay(TOF_INTERMEDIATE_SETTING_DELAY_MS);
+
+        const int8_t boot = sensor->checkBootState();
 
         // change address
         sensor->setI2CAddress(lidar_data.address[sensor_id]);
-
-        PRINTF("%d [#%d] Sensor Boot Code: %d \n", sensor_id, lidar_data.address[sensor_id], boot);
+        PRINTF("[ DEV:ToF ] Sensor[%d] [#%d] Boot Code: %d \n", sensor_id, lidar_data.address[sensor_id], boot);
 
         // activate sensor
-        sensor->init();
+        const int8_t status = sensor->init(); // TODO: get if successful
+        PRINTF("[ DEV:ToF ] Sensor[%d] [#%d] Init Code: %d \n", sensor_id, lidar_data.address[sensor_id], status);
 
         // set initial values
         sensor->setDistanceModeShort();
@@ -188,6 +198,7 @@ void dev_ToF_reset_all_sensors(void)
         // begin firing
         sensor->startRanging();
         delay(TOF_INTERMEDIATE_SETTING_DELAY_MS);
+        sensor->clearInterrupt();
     }
 }
 
@@ -196,8 +207,11 @@ void dev_ToF_reset_all_sensors(void)
 ///////////////////////////////////////
 void dev_ToF_Lidar_init(void)
 {
+    // release semaphore
+    xSemaphoreGive(lidar_data.mp_mutex);
+
     // start tof I2C
-    lidar_data.I2C.begin(TOF_I2C_SDA, TOF_I2C_SCL);
+    lidar_data.I2C.begin(TOF_I2C_SDA, TOF_I2C_SCL, TOF_I2C_FREQ);
 
     // reset ToF
     dev_ToF_reset_all_sensors();
@@ -216,9 +230,9 @@ void dev_ToF_Lidar_update20ms(void)
     uint8_t firing_frame;
     uint8_t firing_frame_new;
     uint8_t geo_label;
-    
+
     // firing
-    for (uint8_t sensor_id = DEV_TOF_LIDAR_R; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
+    for (uint8_t sensor_id = 0U; sensor_id < TOF_SENSOR_COUNT; sensor_id ++)
     {
         sensor = & (lidar_data.tofs[sensor_id]);
         // if data ready
@@ -237,7 +251,7 @@ void dev_ToF_Lidar_update20ms(void)
             }
 
 # if (DEBUG_FPRINT_FEATURE_LIDAR)
-            PRINTF("%d Sensor: %3d [mm] F:[%d] Label:(%2d) Status:(%d) \n", sensor_id, dist_mm, firing_frame, lidar_data.firing_sequence_label[sensor_id][firing_frame], error);
+            PRINTF("[ DEV:TOF ] Sensor[%d]: %3d [mm] F:[%d] Label:(%2d) Status:(%d) \n", sensor_id, dist_mm, firing_frame, lidar_data.firing_sequence_label[sensor_id][firing_frame], error);
 # endif
             // update new firing pattern
             if (sensor->setCenter(lidar_data.firing_sequence[firing_frame_new]) == VL53L1_ERROR_NONE)
@@ -247,7 +261,7 @@ void dev_ToF_Lidar_update20ms(void)
             sensor->clearInterrupt();
 
             // store data
-            if (error == VL53L1_ERROR_NONE)
+            if ((error == DEV_TOF_RANGE_STATUS_SIGNAL_FAILURE) || (error == DEV_TOF_RANGE_STATUS_NO_ERROR))
             {
                 geo_label = lidar_data.firing_sequence_label[sensor_id][firing_frame];
                 if (xSemaphoreTake(lidar_data.mp_mutex, MP_MUTEX_BLOCK_TIME_MS) == pdTRUE) {

Firmware/ESP32/Arduino_platform/lib/DEV/dev_ToF_Lidar.h

@@ -30,9 +30,9 @@ typedef enum{
 } DEV_TOF_FIRING_KEYFRAME_E;
 
 typedef enum{
-    DEV_TOF_LIDAR_R = (0U),
-    DEV_TOF_LIDAR_C,
+    DEV_TOF_LIDAR_C = (0U),
     DEV_TOF_LIDAR_L,
+    DEV_TOF_LIDAR_R,
     DEV_TOF_LIDAR_COUNT,
     DEV_TOF_LIDAR_UNKNOWN
 } DEV_TOF_LIDAR_E;
@@ -57,6 +57,33 @@ typedef enum{ // Geometrical Region 1 -> 15
     DEV_TOF_FIRING_GEOMETRICAL_UNKNOWN,
 } DEV_TOF_FIRING_GEOMETRICAL_REGION_E;
 
+/**
+ *  @brief There are five range statuses: 0, 1, 2, 4, and 7. 
+ *       Range status is 0      => no error. 
+ *       Range status 1 and 2   => error warnings
+ *       Range status 4 and 7   => errors.
+ *  @details Range status:
+ *          1: (sigma failure) => repeatability or standard deviation of the measurement is bad due to a decreasing signal noise ratio. 
+ *              Increasing the timing budget can improve the standard deviation and avoid a range status 1.
+ *          2: (signal failure)=> This means that the return signal is too week to return a good answer. 
+ *             The reason is because the target is too far, or the target is not reflective enough, or the target is too small. 
+ *              Increasing the timing budget might help, but there may simply be no target available.
+ *          4: (out of bounds)=> This means that the sensor is ranging in a “nonappropriated” zone and the measured result may be inconsistent.
+ *             This status is considered as a warning but, in general, it happens when a target is at the maximum distance possible from the sensor, i.e. around 5 m. 
+ *              However, this is only for very bright targets.
+ *          7: (wraparound)=> This situation may occur when the target is very reflective and the distance to the target/sensor is longer than the physical limited distance measurable by the sensor.
+ *             Such distances include approximately 5 m when the senor is in Long distance mode and approximately 1.3 m when the sensor is in Short distance mode.
+ *              Example: a traffic sign located at 6 m can be seen by the sensor and returns a range of 1 m. This is due to “radar aliasing”: 
+ *              if only an approximate distance is required, we may add 6 m to the distance returned. However, that is a very approximate estimation.
+ */
+
+typedef enum{
+    DEV_TOF_RANGE_STATUS_NO_ERROR               = 0,
+    DEV_TOF_RANGE_STATUS_SIGMA_FAILURE          = 1,
+    DEV_TOF_RANGE_STATUS_SIGNAL_FAILURE         = 2,
+    DEV_TOF_RANGE_STATUS_OUT_OF_BOUNDS_FAILURE  = 4,
+    DEV_TOF_RANGE_STATUS_WRAPAROUND_FAILURE     = 7,
+} DEV_TOF_RANGE_STATUS_E;
 
 #define DEV_TOF_TOTAL_POINTS_PER_SCAN       (DEV_TOF_LIDAR_COUNT * DEV_TOF_FIRING_KEYFRAME_COUNT)
 #define DEV_TOF_BUFFER_SCALE                (2U) // 2x buffer shall be enough to cover