diff --git a/components/roode/__init__.py b/components/roode/__init__.py index a55a3f6c..36c6a913 100644 --- a/components/roode/__init__.py +++ b/components/roode/__init__.py @@ -147,7 +147,7 @@ async def to_code(config): await cg.register_component(hub, config) cg.add_library("EEPROM", None) cg.add_library("Wire", None) - cg.add_library("pololu", "1.3.0", "VL53L1X") + cg.add_library("rneurink", "1.2.3", "VL53L1X_ULD") validate_roi_settings(config) diff --git a/components/roode/roode.cpp b/components/roode/roode.cpp index 7990c88f..f969bb92 100644 --- a/components/roode/roode.cpp +++ b/components/roode/roode.cpp @@ -24,23 +24,28 @@ namespace esphome EEPROM.begin(EEPROM_SIZE); Wire.begin(); Wire.setClock(400000); - distanceSensor.setBus(&Wire); - if (distanceSensor.getAddress() != address_) + + // Initialize the sensor, give the special I2C_address to the Begin function + sensor_status = distanceSensor.Begin(VL53L1X_ULD_I2C_ADDRESS); + if (sensor_status != VL53L1_ERROR_NONE) { - distanceSensor.setAddress(address_); + // If the sensor could not be initialized print out the error code. -7 is timeout + ESP_LOGE(SETUP, "Could not initialize the sensor, error code: %d", sensor_status); + while (1) + { + } } + + // 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 (invert_direction_) { ESP_LOGD(TAG, "Inverting direction"); LEFT = 1; RIGHT = 0; } - - distanceSensor.setTimeout(500); - if (!distanceSensor.init()) - { - ESP_LOGE(SETUP, "Failed to detect and initialize sensor!"); - } if (calibration_active_) { calibration(distanceSensor); @@ -50,7 +55,7 @@ namespace esphome { center[0] = 167; center[1] = 231; - distanceSensor.setROISize(Roode::roi_width_, Roode::roi_height_); + distanceSensor.SetROI(Roode::roi_width_, Roode::roi_height_); setSensorMode(sensor_mode, timing_budget_); DIST_THRESHOLD_MAX[0] = Roode::manual_threshold_; DIST_THRESHOLD_MAX[1] = Roode::manual_threshold_; @@ -65,7 +70,8 @@ namespace esphome ESP_LOGD("Roode setup", "last value: %u", peopleCounter); } sendCounter(peopleCounter); - distanceSensor.startContinuous(delay_between_measurements); + distanceSensor.SetInterMeasurementInMs(delay_between_measurements); + distanceSensor.StartRanging(); } void Roode::update() @@ -90,23 +96,49 @@ namespace esphome bool Roode::handleSensorStatus() { - const char *statusString = VL53L1X::rangeStatusToString(sensor_status); // This function call will manipulate the range_status variable ESP_LOGD(TAG, "Sensor status: %d, Last sensor status: %d", sensor_status, last_sensor_status); - - if (last_sensor_status == sensor_status && sensor_status == 0) + bool check_status = false; + if (last_sensor_status == sensor_status && sensor_status == VL53L1_ERROR_NONE) { if (status_sensor != nullptr) { - status_sensor->publish_state(statusString); + status_sensor->publish_state(sensor_status); } - return true; + check_status = true; + } + if (sensor_status < 28 && sensor_status != VL53L1_ERROR_NONE) + { + ESP_LOGE(TAG, "Ranging failed with an error. status: %d", sensor_status); + status_sensor->publish_state(sensor_status); + check_status = false; } - if (sensor_status != 0 && sensor_status != 2 && sensor_status != 7) + + last_sensor_status = sensor_status; + sensor_status = VL53L1_ERROR_NONE; + return check_status; + } + + uint16_t Roode::getDistance() + { + // Checking if data is available. This can also be done through the hardware interrupt. See the ReadDistanceHardwareInterrupt for an example + uint8_t dataReady = false; + while (!dataReady) { - ESP_LOGE(TAG, "Ranging failed with an error. status: %d, error: %s", sensor_status, statusString); - return false; + status += distanceSensor.CheckForDataReady(&dataReady); + delay(1); } - return true; + + // 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; } void Roode::getZoneDistance() @@ -119,12 +151,11 @@ namespace esphome int CurrentZoneStatus = NOBODY; int AllZonesCurrentStatus = 0; int AnEventHasOccured = 0; - distanceSensor.setROICenter(center[zone]); - distanceSensor.startContinuous(delay_between_measurements); + sensor_status += distanceSensor.SetROICenter(center[zone]); + sensor_status += distanceSensor.StartRanging(); last_sensor_status = sensor_status; - distance = distanceSensor.read(); - distanceSensor.writeReg(distanceSensor.SYSTEM__MODE_START, 0x80); // stop reading - sensor_status = distanceSensor.ranging_data.range_status; + distance = getDistance(); + sensor_status += distanceSensor.StopRanging(); if (!handleSensorStatus()) { return; @@ -318,7 +349,7 @@ namespace esphome { calibration(distanceSensor); } - void Roode::roi_calibration(VL53L1X distanceSensor, int optimized_zone_0, int optimized_zone_1) + void Roode::roi_calibration(VL53L1X_ULD distanceSensor, int optimized_zone_0, int optimized_zone_1) { // the value of the average distance is used for computing the optimal size of the ROI and consequently also the center of the two zones int function_of_the_distance = 16 * (1 - (0.15 * 2) / (0.34 * (min(optimized_zone_0, optimized_zone_1) / 1000))); @@ -383,21 +414,27 @@ namespace esphome zone = 0; int *values_zone_0 = new int[number_attempts]; int *values_zone_1 = new int[number_attempts]; - distanceSensor.writeReg(distanceSensor.SYSTEM__MODE_START, 0x80); - distanceSensor.setROISize(Roode::roi_width_, Roode::roi_height_); - distanceSensor.startContinuous(delay_between_measurements); + sensor_status += distanceSensor.StopRanging(); + sensor_status += distanceSensor.SetROI(Roode::roi_width_, Roode::roi_height_); + sensor_status += distanceSensor.SetInterMeasurementInMs(delay_between_measurements); + sensor_status += distanceSensor.StartRanging(); + if (sensor_status != VL53L1_ERROR_NONE) + { + ESP_LOGE(SETUP, "Error in calibration: %d", sensor_status); + } + for (int i = 0; i < number_attempts; i++) { // increase sum of values in Zone 0 - distanceSensor.setROICenter(center[zone]); - distance = distanceSensor.read(); + distanceSensor.SetROICenter(center[zone]); + distance = getDistance(); values_zone_0[i] = distance; zone++; zone = zone % 2; App.feed_wdt(); // increase sum of values in Zone 1 - distanceSensor.setROICenter(center[zone]); - distance = distanceSensor.read(); + distanceSensor.SetROICenter(center[zone]); + distance = getDistance(); values_zone_1[i] = distance; zone++; zone = zone % 2; @@ -407,54 +444,54 @@ namespace esphome } void Roode::setSensorMode(int sensor_mode, int new_timing_budget) { - distanceSensor.writeReg(distanceSensor.SYSTEM__MODE_START, 0x80); + distanceSensor.StopRanging(); switch (sensor_mode) { case 0: // short mode time_budget_in_ms = time_budget_in_ms_short; delay_between_measurements = time_budget_in_ms + 5; - status = distanceSensor.setDistanceMode(VL53L1X::Short); - if (!status) + sensor_status = distanceSensor.SetDistanceMode(Short); + if (sensor_status != VL53L1_ERROR_NONE) { - ESP_LOGE(SETUP, "Could not set distance mode. mode: %d", VL53L1X::Short); + ESP_LOGE(SETUP, "Could not set distance mode. mode: %d", Short); } ESP_LOGI(SETUP, "Set short mode. timing_budget: %d", time_budget_in_ms); break; case 1: // medium mode time_budget_in_ms = time_budget_in_ms_medium; delay_between_measurements = time_budget_in_ms + 5; - status = distanceSensor.setDistanceMode(VL53L1X::Medium); - if (!status) + sensor_status = distanceSensor.SetDistanceMode(Long); + if (sensor_status != VL53L1_ERROR_NONE) { - ESP_LOGE(SETUP, "Could not set distance mode. mode: %d", VL53L1X::Medium); + ESP_LOGE(SETUP, "Could not set distance mode. mode: %d", Long); } ESP_LOGI(SETUP, "Set medium mode. timing_budget: %d", time_budget_in_ms); break; case 2: // long mode time_budget_in_ms = time_budget_in_ms_long; delay_between_measurements = time_budget_in_ms + 5; - status = distanceSensor.setDistanceMode(VL53L1X::Long); - if (!status) + sensor_status = distanceSensor.SetDistanceMode(Long); + if (sensor_status != VL53L1_ERROR_NONE) { - ESP_LOGE(SETUP, "Could not set distance mode. mode: %d", VL53L1X::Long); + ESP_LOGE(SETUP, "Could not set distance mode. mode: %d", Long); } ESP_LOGI(SETUP, "Set long range mode. timing_budget: %d", time_budget_in_ms); break; case 3: // custom mode time_budget_in_ms = new_timing_budget; delay_between_measurements = new_timing_budget + 5; - status = distanceSensor.setDistanceMode(VL53L1X::Long); - if (!status) + sensor_status = distanceSensor.SetDistanceMode(Long); + if (sensor_status != VL53L1_ERROR_NONE) { - ESP_LOGE(SETUP, "Could not set distance mode. mode: %d", VL53L1X::Long); + ESP_LOGE(SETUP, "Could not set distance mode. mode: %d", Long); } ESP_LOGI(SETUP, "Manually set custom range mode. timing_budget: %d", time_budget_in_ms); break; default: break; } - status = distanceSensor.setMeasurementTimingBudget(time_budget_in_ms * 1000); - if (!status) + sensor_status = distanceSensor.SetTimingBudgetInMs(time_budget_in_ms); + if (sensor_status != 0) { ESP_LOGE(SETUP, "Could not set timing budget. timing_budget: %d ms", time_budget_in_ms); } @@ -476,10 +513,10 @@ namespace esphome { setSensorMode(2, time_budget_in_ms_long); } - status = distanceSensor.setMeasurementTimingBudget(time_budget_in_ms * 1000); - if (!status) + sensor_status = distanceSensor.SetTimingBudgetInMs(time_budget_in_ms); + if (sensor_status != VL53L1_ERROR_NONE) { - ESP_LOGE(CALIBRATION, "Could not set timing budget. timing_budget: %d ms", time_budget_in_ms); + ESP_LOGE(CALIBRATION, "Could not set timing budget. timing_budget: %d ms, status: %d", time_budget_in_ms, sensor_status); } } int Roode::getSum(int *array, int size) @@ -511,18 +548,18 @@ namespace esphome return avg - sd; } - void Roode::calibration(VL53L1X distanceSensor) + void Roode::calibration(VL53L1X_ULD distanceSensor) { - distanceSensor.writeReg(distanceSensor.SYSTEM__MODE_START, 0x80); + distanceSensor.StopRanging(); // the sensor does 100 measurements for each zone (zones are predefined) time_budget_in_ms = time_budget_in_ms_medium; delay_between_measurements = time_budget_in_ms + 5; - distanceSensor.setDistanceMode(VL53L1X::Medium); - status = distanceSensor.setMeasurementTimingBudget(time_budget_in_ms * 1000); + distanceSensor.SetDistanceMode(Long); + sensor_status = distanceSensor.SetTimingBudgetInMs(time_budget_in_ms); - if (!status) + if (sensor_status != VL53L1_ERROR_NONE) { - ESP_LOGE(CALIBRATION, "Could not set timing budget. timing_budget: %d ms", time_budget_in_ms); + ESP_LOGE(CALIBRATION, "Could not set timing budget. timing_budget: %d ms, status: %d", time_budget_in_ms, sensor_status); } if (advised_sensor_orientation_) { @@ -543,20 +580,21 @@ namespace esphome int *values_zone_0 = new int[number_attempts]; int *values_zone_1 = new int[number_attempts]; - distanceSensor.setROISize(Roode::roi_width_, Roode::roi_height_); - distanceSensor.startContinuous(delay_between_measurements); + distanceSensor.SetROI(Roode::roi_width_, Roode::roi_height_); + distanceSensor.SetInterMeasurementInMs(delay_between_measurements); + distanceSensor.StartRanging(); for (int i = 0; i < number_attempts; i++) { // increase sum of values in Zone 0 - distanceSensor.setROICenter(center[zone]); - distance = distanceSensor.read(); + distanceSensor.SetROICenter(center[zone]); + distance = getDistance(); values_zone_0[i] = distance; zone++; zone = zone % 2; App.feed_wdt(); // increase sum of values in Zone 1 - distanceSensor.setROICenter(center[zone]); - distance = distanceSensor.read(); + distanceSensor.SetROICenter(center[zone]); + distance = getDistance(); values_zone_1[i] = distance; zone++; zone = zone % 2; @@ -586,7 +624,7 @@ namespace esphome DIST_THRESHOLD_MIN[1] = optimized_zone_1 * min_threshold_percentage_ / 100; publishSensorConfiguration(DIST_THRESHOLD_MIN, false); } - distanceSensor.writeReg(distanceSensor.SYSTEM__MODE_START, 0x80); + distanceSensor.StopRanging(); } void Roode::publishSensorConfiguration(int DIST_THRESHOLD_ARR[2], bool isMax) diff --git a/components/roode/roode.h b/components/roode/roode.h index 353b06fe..b51fa447 100644 --- a/components/roode/roode.h +++ b/components/roode/roode.h @@ -6,7 +6,8 @@ #include "esphome/components/i2c/i2c.h" #include "esphome/core/application.h" #include "EEPROM.h" -#include +// #include +#include "VL53L1X_ULD.h" #include namespace esphome @@ -15,8 +16,9 @@ namespace esphome { #define NOBODY 0 #define SOMEONE 1 -#define VERSION "v1.4.0-beta" +#define VERSION "v1.4.1-beta" #define EEPROM_SIZE 512 +#define VL53L1X_ULD_I2C_ADDRESS 0x52 // Default address is 0x52 static int LEFT = 0; static int RIGHT = 1; static const uint16_t DISTANCES_ARRAY_SIZE = 2; @@ -43,10 +45,11 @@ namespace esphome The timing budget and inter-measurement period should not be called when the sensor is ranging. The user has to stop the ranging, change these parameters, and restart ranging The minimum inter-measurement period must be longer than the timing budget + 4 ms. + Valid values: [15,20,33,50,100,200,500] */ - static int time_budget_in_ms_short = 20; // 20ms with the full API but 15ms with the ULD API (https://www.st.com/resource/en/user_manual/um2510-a-guide-to-using-the-vl53l1x-ultra-lite-driver-stmicroelectronics.pdf) - static int time_budget_in_ms_medium = 60; // Works up to 3.1m increase to minimum of 140ms for 4m - static int time_budget_in_ms_long = 140; // Works up to 4m in the dark on a white chart + static int time_budget_in_ms_short = 15; // Lowest possible is 15ms with the ULD API (https://www.st.com/resource/en/user_manual/um2510-a-guide-to-using-the-vl53l1x-ultra-lite-driver-stmicroelectronics.pdf) + static int time_budget_in_ms_medium = 33; // Works up to 3.1m + static int time_budget_in_ms_long = 100; // Works up to 4m in the dark on a white chart class Roode : public PollingComponent { @@ -79,19 +82,20 @@ namespace esphome void set_min_threshold_zone1_sensor(sensor::Sensor *min_threshold_zone1_sensor_) { min_threshold_zone1_sensor = min_threshold_zone1_sensor_; } void set_roi_height_sensor(sensor::Sensor *roi_height_sensor_) { roi_height_sensor = roi_height_sensor_; } void set_roi_width_sensor(sensor::Sensor *roi_width_sensor_) { roi_width_sensor = roi_width_sensor_; } + void set_sensor_status_sensor(sensor::Sensor *status_sensor_) { status_sensor = status_sensor_; } void set_presence_sensor_binary_sensor(binary_sensor::BinarySensor *presence_sensor_) { presence_sensor = presence_sensor_; } void set_version_text_sensor(text_sensor::TextSensor *version_sensor_) { version_sensor = version_sensor_; } void set_entry_exit_event_text_sensor(text_sensor::TextSensor *entry_exit_event_sensor_) { entry_exit_event_sensor = entry_exit_event_sensor_; } - void set_sensor_status_text_sensor(text_sensor::TextSensor *status_sensor_) { status_sensor = status_sensor_; } void set_sensor_mode(int sensor_mode_) { sensor_mode = sensor_mode_; } void getZoneDistance(); void sendCounter(uint16_t counter); void recalibration(); bool handleSensorStatus(); + uint16_t getDistance(); uint16_t distance = 0; - VL53L1X::RangeStatus last_sensor_status = VL53L1X::RangeStatus::None; - VL53L1X::RangeStatus sensor_status = VL53L1X::RangeStatus::None; + VL53L1_Error last_sensor_status = VL53L1_ERROR_NONE; + VL53L1_Error sensor_status = VL53L1_ERROR_NONE; int DIST_THRESHOLD_MAX[2] = {0, 0}; // max treshold of the two zones int DIST_THRESHOLD_MIN[2] = {0, 0}; // min treshold of the two zones int roi_width_{6}; // width of the ROI @@ -99,7 +103,7 @@ namespace esphome uint16_t peopleCounter{0}; protected: - VL53L1X distanceSensor; + VL53L1X_ULD distanceSensor; sensor::Sensor *distance_sensor; sensor::Sensor *people_counter_sensor; sensor::Sensor *max_threshold_zone0_sensor; @@ -108,13 +112,13 @@ namespace esphome sensor::Sensor *min_threshold_zone1_sensor; sensor::Sensor *roi_height_sensor; sensor::Sensor *roi_width_sensor; + sensor::Sensor *status_sensor; binary_sensor::BinarySensor *presence_sensor; text_sensor::TextSensor *version_sensor; text_sensor::TextSensor *entry_exit_event_sensor; - text_sensor::TextSensor *status_sensor; - void roi_calibration(VL53L1X distanceSensor, int optimized_zone_0, int optimized_zone_1); - void calibration(VL53L1X distanceSensor); + void roi_calibration(VL53L1X_ULD distanceSensor, int optimized_zone_0, int optimized_zone_1); + void calibration(VL53L1X_ULD distanceSensor); void setCorrectDistanceSettings(float average_zone_0, float average_zone_1); void setSensorMode(int sensor_mode, int timing_budget = 0); void publishSensorConfiguration(int DIST_THRESHOLD_ARR[2], bool isMax); diff --git a/components/roode/sensor.py b/components/roode/sensor.py index c93bcfa8..cb464ddf 100644 --- a/components/roode/sensor.py +++ b/components/roode/sensor.py @@ -22,6 +22,8 @@ CONF_MIN_THRESHOLD_ZONE1 = "min_threshold_zone1" CONF_ROI_HEIGHT = "roi_height" CONF_ROI_WIDTH = "roi_width" +SENSOR_STATUS = "sensor_status" + CONFIG_SCHEMA = sensor.sensor_schema().extend( { cv.Optional(CONF_DISTANCE): sensor.sensor_schema( @@ -79,6 +81,11 @@ state_class=STATE_CLASS_MEASUREMENT, entity_category=ENTITY_CATEGORY_DIAGNOSTIC, ), + cv.Optional(SENSOR_STATUS): sensor.sensor_schema( + icon="mdi:check-circle", + accuracy_decimals=0, + entity_category=ENTITY_CATEGORY_DIAGNOSTIC, + ), cv.GenerateID(CONF_ROODE_ID): cv.use_id(Roode), } ) @@ -110,3 +117,6 @@ async def to_code(config): if CONF_ROI_WIDTH in config: count = await sensor.new_sensor(config[CONF_ROI_WIDTH]) cg.add(var.set_roi_width_sensor(count)) + if SENSOR_STATUS in config: + count = await sensor.new_sensor(config[SENSOR_STATUS]) + cg.add(var.set_sensor_status_sensor(count)) diff --git a/components/roode/text_sensor.py b/components/roode/text_sensor.py index 0889f041..d20cd24d 100644 --- a/components/roode/text_sensor.py +++ b/components/roode/text_sensor.py @@ -38,15 +38,6 @@ ): cv.entity_category, } ), - cv.Optional(STATUS): text_sensor.TEXT_SENSOR_SCHEMA.extend( - { - cv.Optional(CONF_ICON, default="mdi:check-circle"): text_sensor.icon, - cv.GenerateID(): cv.declare_id(text_sensor.TextSensor), - cv.Optional( - CONF_ENTITY_CATEGORY, default=ENTITY_CATEGORY_DIAGNOSTIC - ): cv.entity_category, - } - ), } ) diff --git a/peopleCounter.yaml b/peopleCounter.yaml index b3f22413..fbaa6819 100644 --- a/peopleCounter.yaml +++ b/peopleCounter.yaml @@ -119,6 +119,8 @@ sensor: name: $friendly_name ROI height roi_width: name: $friendly_name ROI width + sensor_status: + name: Sensor Status - platform: wifi_signal name: $friendly_name RSSI @@ -152,7 +154,6 @@ text_sensor: - platform: roode version: name: $friendly_name version - - platform: roode entry_exit_event: name: $friendly_name last direction - platform: template diff --git a/peopleCounter32.yaml b/peopleCounter32.yaml index 4f2d897d..bf9d948e 100644 --- a/peopleCounter32.yaml +++ b/peopleCounter32.yaml @@ -122,7 +122,9 @@ sensor: name: $friendly_name ROI height roi_width: name: $friendly_name ROI width - + sensor_status: + name: Sensor Status + - platform: wifi_signal name: $friendly_name RSSI update_interval: 60s diff --git a/peopleCounterDev.yaml b/peopleCounterDev.yaml index 92fb8c03..15c923b8 100644 --- a/peopleCounterDev.yaml +++ b/peopleCounterDev.yaml @@ -81,7 +81,7 @@ switch: name: $friendly_name Restart binary_sensor: - platform: status - name: $friendly_name Status + name: $friendly_name API Status - platform: roode presence_sensor: name: $friendly_name presence @@ -108,6 +108,8 @@ sensor: name: $friendly_name ROI height roi_width: name: $friendly_name ROI width + sensor_status: + name: Sensor Status - platform: wifi_signal name: $friendly_name RSSI