Merge pull request #79 from Lyr3x/add-calibration-code

Add calibration code and all timing budgets

calibration/.vscode/extensions.json

@@ -0,0 +1,7 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ]
+}

calibration/.vscode/launch.json

@@ -0,0 +1,44 @@
+// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY
+//
+// PIO Unified Debugger
+//
+// Documentation: https://docs.platformio.org/page/plus/debugging.html
+// Configuration: https://docs.platformio.org/page/projectconf/section_env_debug.html
+
+{
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "type": "platformio-debug",
+            "request": "launch",
+            "name": "PIO Debug",
+            "executable": "/Users/kaibepperling/dev/Roode/calibration/.pio/build/roode-calibration/firmware.elf",
+            "projectEnvName": "roode-calibration",
+            "toolchainBinDir": "/Users/kaibepperling/.platformio/packages/toolchain-xtensa32@2.50200.97/bin",
+            "internalConsoleOptions": "openOnSessionStart",
+            "preLaunchTask": {
+                "type": "PlatformIO",
+                "task": "Pre-Debug"
+            }
+        },
+        {
+            "type": "platformio-debug",
+            "request": "launch",
+            "name": "PIO Debug (skip Pre-Debug)",
+            "executable": "/Users/kaibepperling/dev/Roode/calibration/.pio/build/roode-calibration/firmware.elf",
+            "projectEnvName": "roode-calibration",
+            "toolchainBinDir": "/Users/kaibepperling/.platformio/packages/toolchain-xtensa32@2.50200.97/bin",
+            "internalConsoleOptions": "openOnSessionStart"
+        },
+        {
+            "type": "platformio-debug",
+            "request": "launch",
+            "name": "PIO Debug (without uploading)",
+            "executable": "/Users/kaibepperling/dev/Roode/calibration/.pio/build/roode-calibration/firmware.elf",
+            "projectEnvName": "roode-calibration",
+            "toolchainBinDir": "/Users/kaibepperling/.platformio/packages/toolchain-xtensa32@2.50200.97/bin",
+            "internalConsoleOptions": "openOnSessionStart",
+            "loadMode": "manual"
+        }
+    ]
+}

calibration/OffsetAndXtalkCalibration/OffsetAndXtalkCalibration.ino

@@ -0,0 +1,62 @@
+#include "VL53L1X_ULD.h"
+
+VL53L1X_ULD sensor;
+bool dataReady;
+
+void setup()
+{
+    Serial.begin(115200);
+    Wire.begin();
+
+    VL53L1_Error sensor_status = sensor.Begin();
+    if (sensor_status != VL53L1_ERROR_NONE)
+    {
+        Serial.println("Could not initialize the sensor, error code: " + String(sensor_status));
+        while (1)
+        {
+        }
+    }
+    Serial.println("Sensor initialized");
+
+    // Start the offset calibration
+    Serial.println("Place a target, 17 % grey, at 140 mm from the sensor a");
+    Serial.println("The calibration may take a few seconds. The offset correction is applied to the sensor at the end of calibration.");
+    Serial.readString();
+    int16_t foundOffset;
+    sensor_status = sensor.CalibrateOffset(140, &foundOffset);
+
+
+    Serial.println("Calibrated offset: " + String(foundOffset));
+    Serial.println("Set this offset in the sensor configuration under offset: <value>");
+
+    /* The target distance : the distance where the sensor start to "under range"
+    Crosstalk calibration should be conducted in a dark environment, with no IR contribution.
+    The crosstalk calibration distance needs to be characterized as it depends on the system environment which
+    mainly includes:
+    • The cover glass material and optical properties
+    • The air gap value i.e. the distance between the sensor and the cover glass
+    Do a full sweep with the target from near to far, noting the resulting measurement.
+    At some point, the actual value and the measured value start to diverge. This is the crosstalk calibration
+    distance.
+    */
+    uint16_t CalibrationDistance = 140; // crosstalk calibration distance
+    uint16_t foundXTalk;
+    sensor_status = sensor.CalibrateXTalk(CalibrationDistance, &foundXTalk);
+    Serial.println("Calibrated offset: " + String(foundXTalk));
+    Serial.println("Set this offset in the sensor configuration under crosstalk: <value>");
+}
+void loop()
+{
+    if (dataReady)
+    {
+        // Get the results
+        uint16_t distance;
+        sensor.GetDistanceInMm(&distance);
+
+        // After reading the results reset the interrupt to be able to take another measurement
+        sensor.ClearInterrupt();
+        dataReady = false;
+
+        Serial.println("Distance in mm: " + String(distance));
+    }
+}

calibration/platformio.ini

@@ -0,0 +1,16 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:roode-calibration]
+platform = espressif32
+board = wemos_d1_mini32
+framework = arduino
+lib_deps = rneurink/VL53L1X_ULD@^1.2.3
+monitor_speed = 115200

calibration/src/OffsetAndXtalkCalibration.cpp

@@ -0,0 +1,65 @@
+#include "VL53L1X_ULD.h"
+
+VL53L1X_ULD sensor;
+bool dataReady;
+
+void setup()
+{
+    Serial.begin(115200);
+    Wire.begin();
+
+    VL53L1_Error sensor_status = sensor.Begin();
+    if (sensor_status != VL53L1_ERROR_NONE)
+    {
+        Serial.println("Could not initialize the sensor, error code: " + String(sensor_status));
+        while (1)
+        {
+        }
+    }
+    Serial.println("Sensor initialized");
+
+    // Start the offset calibration
+    Serial.println("Place a target, 17 % grey, at 140 mm from the sensor a");
+    Serial.println("The calibration may take a few seconds. The offset correction is applied to the sensor at the end of calibration.");
+    Serial.readString();
+    int16_t foundOffset;
+    sensor_status = sensor.CalibrateOffset(140, &foundOffset);
+
+    sensor.SetOffsetInMm(foundOffset);
+
+    Serial.println("Calibrated offset: " + String(foundOffset));
+    Serial.printf("Set this offset in the sensor configuration under offset: %d\n", foundOffset);
+
+    /* The target distance : the distance where the sensor start to "under range"
+    Crosstalk calibration should be conducted in a dark environment, with no IR contribution.
+    The crosstalk calibration distance needs to be characterized as it depends on the system environment which
+    mainly includes:
+    • The cover glass material and optical properties
+    • The air gap value i.e. the distance between the sensor and the cover glass
+    Do a full sweep with the target from near to far, noting the resulting measurement.
+    At some point, the actual value and the measured value start to diverge. This is the crosstalk calibration
+    distance.
+    */
+    uint16_t CalibrationDistance = 140; // crosstalk calibration distance
+    uint16_t foundXTalk;
+    sensor_status = sensor.CalibrateXTalk(CalibrationDistance, &foundXTalk);
+    Serial.println("Calibrated offset: " + String(foundXTalk));
+    Serial.printf("Set this offset in the sensor configuration under crosstalk: %d\n", foundXTalk);
+    sensor.SetXTalk(foundXTalk);
+    sensor.StartRanging();
+}
+void loop()
+{
+    if (dataReady)
+    {
+        // Get the results
+        uint16_t distance;
+        sensor.GetDistanceInMm(&distance);
+
+        // After reading the results reset the interrupt to be able to take another measurement
+        sensor.ClearInterrupt();
+        dataReady = false;
+
+        Serial.println("Distance in mm: " + String(distance));
+    }
+}

components/roode/__init__.py

@@ -40,6 +40,8 @@
 CONF_USE_SAMPLING = "use_sampling"
 CONF_ROI = "roi"
 CONF_ROI_ACTIVE = "roi_active"
+CONF_SENSOR_OFFSET_CALIBRATION = "sensor_offset_calibration"
+CONF_SENSOR_XTALK_CALIBRATION = "sensor_xtalk_calibration"
 TYPES = [
     CONF_RESTORE_VALUES,
     CONF_INVERT_DIRECTION,
@@ -69,6 +71,8 @@
                     min=0, max=100
                 ),
                 cv.Optional(CONF_ROI_CALIBRATION, default="false"): cv.boolean,
+                cv.Optional(CONF_SENSOR_OFFSET_CALIBRATION, default=-1): cv.int_,
+                cv.Optional(CONF_SENSOR_XTALK_CALIBRATION, default=-1): cv.int_,
             }
         ),
         cv.Exclusive(
@@ -148,7 +152,7 @@ async def to_code(config):
     cg.add_library("EEPROM", None)
     cg.add_library("Wire", None)
     cg.add_library("rneurink", "1.2.3", "VL53L1X_ULD")
-    
+
     validate_roi_settings(config)
 
     for key in TYPES:

components/roode/roode.cpp

@@ -26,6 +26,10 @@ namespace esphome
             Wire.setClock(400000);
 
             // Initialize the sensor, give the special I2C_address to the Begin function
+            // Set a different I2C address
+            // This address is stored as long as the sensor is powered. To revert this change you can unplug and replug the power to the sensor
+            distanceSensor.SetI2CAddress(VL53L1X_ULD_I2C_ADDRESS);
+
             sensor_status = distanceSensor.Begin(VL53L1X_ULD_I2C_ADDRESS);
             if (sensor_status != VL53L1_ERROR_NONE)
             {
@@ -35,10 +39,30 @@ namespace esphome
                 {
                 }
             }
-
-            // Set a different I2C address
-            // This address is stored as long as the sensor is powered. To revert this change you can unplug and replug the power to the sensor
-            distanceSensor.SetI2CAddress(VL53L1X_ULD_I2C_ADDRESS);
+            if (sensor_offset_calibration_ != -1)
+            {
+                ESP_LOGI(CALIBRATION, "Setting sensor offset calibration to %d", sensor_offset_calibration_);
+                sensor_status = distanceSensor.SetOffsetInMm(sensor_offset_calibration_);
+                if (sensor_status != VL53L1_ERROR_NONE)
+                {
+                    ESP_LOGE(SETUP, "Could not set sensor offset calibration, error code: %d", sensor_status);
+                    while (1)
+                    {
+                    }
+                }
+            }
+            if (sensor_xtalk_calibration_ != -1)
+            {
+                ESP_LOGI(CALIBRATION, "Setting sensor xtalk calibration to %d", sensor_xtalk_calibration_);
+                sensor_status = distanceSensor.SetXTalk(sensor_xtalk_calibration_);
+                if (sensor_status != VL53L1_ERROR_NONE)
+                {
+                    ESP_LOGE(SETUP, "Could not set sensor offset calibration, error code: %d", sensor_status);
+                    while (1)
+                    {
+                    }
+                }
+            }
 
             if (invert_direction_)
             {
@@ -124,20 +148,22 @@ namespace esphome
             uint8_t dataReady = false;
             while (!dataReady)
             {
-                status += distanceSensor.CheckForDataReady(&dataReady);
+                sensor_status += distanceSensor.CheckForDataReady(&dataReady);
                 delay(1);
             }
 
             // Get the results
             uint16_t distance;
             sensor_status += distanceSensor.GetDistanceInMm(&distance);
+
             if (sensor_status != VL53L1_ERROR_NONE)
             {
                 ESP_LOGE(TAG, "Could not get distance, error code: %d", sensor_status);
                 return sensor_status;
             }
             // After reading the results reset the interrupt to be able to take another measurement
             distanceSensor.ClearInterrupt();
+
             return distance;
         }
 
@@ -275,14 +301,14 @@ namespace esphome
                             {
                                 peopleCounter--;
                                 sendCounter(peopleCounter);
-                                ESP_LOGD("Roode pathTracking", "Exit detected.");
-                                if (entry_exit_event_sensor != nullptr)
-                                {
-                                    entry_exit_event_sensor->publish_state("Exit");
-                                }
                                 DistancesTableSize[0] = 0;
                                 DistancesTableSize[1] = 0;
                             }
+                            ESP_LOGD("Roode pathTracking", "Exit detected.");
+                            if (entry_exit_event_sensor != nullptr)
+                            {
+                                entry_exit_event_sensor->publish_state("Exit");
+                            }
                         }
                         else if ((PathTrack[1] == 2) && (PathTrack[2] == 3) && (PathTrack[3] == 1))
                         {
@@ -467,7 +493,17 @@ namespace esphome
                 }
                 ESP_LOGI(SETUP, "Set medium mode. timing_budget: %d", time_budget_in_ms);
                 break;
-            case 2: // long mode
+            case 2: // medium_long mode
+                time_budget_in_ms = time_budget_in_ms_medium_long;
+                delay_between_measurements = time_budget_in_ms + 5;
+                sensor_status = distanceSensor.SetDistanceMode(Long);
+                if (sensor_status != VL53L1_ERROR_NONE)
+                {
+                    ESP_LOGE(SETUP, "Could not set distance mode.  mode: %d", Long);
+                }
+                ESP_LOGI(SETUP, "Set medium long range mode. timing_budget: %d", time_budget_in_ms);
+                break;
+            case 3: // long mode
                 time_budget_in_ms = time_budget_in_ms_long;
                 delay_between_measurements = time_budget_in_ms + 5;
                 sensor_status = distanceSensor.SetDistanceMode(Long);
@@ -477,7 +513,17 @@ namespace esphome
                 }
                 ESP_LOGI(SETUP, "Set long range mode. timing_budget: %d", time_budget_in_ms);
                 break;
-            case 3: // custom mode
+            case 4: // max mode
+                time_budget_in_ms = time_budget_in_ms_max;
+                delay_between_measurements = time_budget_in_ms + 5;
+                sensor_status = distanceSensor.SetDistanceMode(Long);
+                if (sensor_status != VL53L1_ERROR_NONE)
+                {
+                    ESP_LOGE(SETUP, "Could not set distance mode.  mode: %d", Long);
+                }
+                ESP_LOGI(SETUP, "Set max range mode. timing_budget: %d", time_budget_in_ms);
+                break;
+            case 5: // custom mode
                 time_budget_in_ms = new_timing_budget;
                 delay_between_measurements = new_timing_budget + 5;
                 sensor_status = distanceSensor.SetDistanceMode(Long);
@@ -501,24 +547,28 @@ namespace esphome
         {
             if (average_zone_0 <= short_distance_threshold || average_zone_1 <= short_distance_threshold)
             {
-                setSensorMode(0, time_budget_in_ms_short);
+                setSensorMode(0);
             }
 
             if ((average_zone_0 > short_distance_threshold && average_zone_0 <= medium_distance_threshold) || (average_zone_1 > short_distance_threshold && average_zone_1 <= medium_distance_threshold))
             {
-                setSensorMode(1, time_budget_in_ms_medium);
+                setSensorMode(1);
             }
 
-            if (average_zone_0 > medium_distance_threshold || average_zone_1 > medium_distance_threshold)
+            if ((average_zone_0 > medium_distance_threshold && average_zone_0 <= medium_long_distance_threshold) || (average_zone_1 > medium_distance_threshold && average_zone_1 <= medium_long_distance_threshold))
             {
-                setSensorMode(2, time_budget_in_ms_long);
+                setSensorMode(2);
             }
-            sensor_status = distanceSensor.SetTimingBudgetInMs(time_budget_in_ms);
-            if (sensor_status != VL53L1_ERROR_NONE)
+            if ((average_zone_0 > medium_long_distance_threshold && average_zone_0 <= long_distance_threshold) || (average_zone_1 > medium_long_distance_threshold && average_zone_1 <= long_distance_threshold))
             {
-                ESP_LOGE(CALIBRATION, "Could not set timing budget. timing_budget: %d ms, status: %d", time_budget_in_ms, sensor_status);
+                setSensorMode(3);
+            }
+            if (average_zone_0 > long_distance_threshold || average_zone_1 > long_distance_threshold)
+            {
+                setSensorMode(4);
             }
         }
+
         int Roode::getSum(int *array, int size)
         {
             int sum = 0;