diff --git a/components/roode/roode.cpp b/components/roode/roode.cpp index 1e1f3983..091430b5 100644 --- a/components/roode/roode.cpp +++ b/components/roode/roode.cpp @@ -180,9 +180,9 @@ void Roode::calibrate_zones() { calibrateDistance(); entry->roi_calibration(entry->threshold->idle, exit->threshold->idle, orientation_); - entry->calibrateThreshold(distanceSensor, number_attempts); + entry->calibrate_threshold(distanceSensor, number_attempts); exit->roi_calibration(entry->threshold->idle, exit->threshold->idle, orientation_); - exit->calibrateThreshold(distanceSensor, number_attempts); + exit->calibrate_threshold(distanceSensor, number_attempts); publish_sensor_configuration(entry, exit, true); App.feed_wdt(); @@ -194,8 +194,8 @@ void Roode::calibrateDistance() { auto *const initial = distanceSensor->get_ranging_mode_override().value_or(Ranging::Longest); distanceSensor->set_ranging_mode(initial); - entry->calibrateThreshold(distanceSensor, number_attempts); - exit->calibrateThreshold(distanceSensor, number_attempts); + entry->calibrate_threshold(distanceSensor, number_attempts); + exit->calibrate_threshold(distanceSensor, number_attempts); if (distanceSensor->get_ranging_mode_override().has_value()) { return; diff --git a/components/roode/zone.cpp b/components/roode/zone.cpp index 5202cedc..90ccdbac 100644 --- a/components/roode/zone.cpp +++ b/components/roode/zone.cpp @@ -29,6 +29,24 @@ VL53L1_Error Zone::readDistance(TofSensor *distanceSensor) { occupancy->publish_state(min_distance < threshold->max && min_distance > threshold->min); return status; + this->update_threshold(min_distance); + return sensor_status; +} + +void Zone::update_threshold(uint16_t distance) { + if (distance > threshold->max) { + idle_distances.insert(idle_distances.begin(), distance); + } + if (idle_distances.size() == 100) { + this->threshold->idle = this->get_avg(idle_distances); + if (this->threshold->max_percentage.has_value()) { + this->threshold->max = (this->threshold->idle * this->threshold->max_percentage.value()) / 100; + } + if (this->threshold->min_percentage.has_value()) { + this->threshold->min = (this->threshold->idle * this->threshold->min_percentage.value()) / 100; + } + idle_distances.clear(); + } } /** @@ -43,7 +61,7 @@ void Zone::reset_roi(uint8_t default_center) { roi->height, roi->center); } -void Zone::calibrateThreshold(TofSensor *distanceSensor, int number_attempts) { +void Zone::calibrate_threshold(TofSensor *distanceSensor, int number_attempts) { ESP_LOGD(CALIBRATION, "Beginning. zoneId: %d", id); int *zone_distances = new int[number_attempts]; int sum = 0; @@ -52,7 +70,7 @@ void Zone::calibrateThreshold(TofSensor *distanceSensor, int number_attempts) { zone_distances[i] = last_distance; sum += zone_distances[i]; }; - threshold->idle = this->getOptimizedValues(zone_distances, sum, number_attempts); + threshold->idle = this->get_optimized_values(zone_distances, sum, number_attempts); if (threshold->max_percentage.has_value()) { threshold->max = (threshold->idle * threshold->max_percentage.value()) / 100; @@ -111,7 +129,7 @@ void Zone::roi_calibration(uint16_t entry_threshold, uint16_t exit_threshold, Or roi->height, roi->center); } -int Zone::getOptimizedValues(int *values, int sum, int size) { +int Zone::get_optimized_values(int *values, int sum, int size) { int sum_squared = 0; int variance = 0; int sd = 0; @@ -133,5 +151,10 @@ void Zone::update() { distance_sensor->publish_state(min_distance); } } +int Zone::get_avg(std::vector values) { + auto sum = std::accumulate(values.begin(), values.end(), 0); + int avg = sum / values.size(); + return avg; +} } // namespace roode -} // namespace esphome \ No newline at end of file +} // namespace esphome diff --git a/components/roode/zone.h b/components/roode/zone.h index 8761194b..a0920f74 100644 --- a/components/roode/zone.h +++ b/components/roode/zone.h @@ -34,7 +34,7 @@ class Zone : public PollingComponent { void update() override; VL53L1_Error readDistance(TofSensor *distanceSensor); void reset_roi(uint8_t default_center); - void calibrateThreshold(TofSensor *distanceSensor, int number_attempts); + void calibrate_threshold(TofSensor *distanceSensor, int number_attempts); void roi_calibration(uint16_t entry_threshold, uint16_t exit_threshold, Orientation orientation); const uint8_t id; bool is_occupied() const { return occupancy->state; }; @@ -47,12 +47,17 @@ class Zone : public PollingComponent { void set_distance_sensor(sensor::Sensor *sensor) { distance_sensor = sensor; } protected: - int getOptimizedValues(int *values, int sum, int size); + int get_optimized_values(int *values, int sum, int size); + int get_avg(std::vector values); + void update_threshold(uint16_t distance); + VL53L1_Error last_sensor_status = VL53L1_ERROR_NONE; + VL53L1_Error sensor_status = VL53L1_ERROR_NONE; uint16_t last_distance; uint16_t min_distance; std::vector samples; uint8_t max_samples; sensor::Sensor *distance_sensor{nullptr}; + std::vector idle_distances; }; } // namespace roode } // namespace esphome diff --git a/peopleCounter32.yaml b/peopleCounter32.yaml index 3d79de41..f3273f94 100644 --- a/peopleCounter32.yaml +++ b/peopleCounter32.yaml @@ -35,16 +35,6 @@ api: - service: recalibrate then: - lambda: "id(roode_platform)->recalibration();" - - service: set_max_threshold - variables: - newThreshold: int - then: - - lambda: "id(roode_platform)->set_max_threshold_percentage(newThreshold);id(roode_platform)->recalibration();" - - service: set_min_threshold - variables: - newThreshold: int - then: - - lambda: "id(roode_platform)->set_min_threshold_percentage(newThreshold);id(roode_platform)->recalibration();" ota: password: !secret ota_password diff --git a/peopleCounter32Dev.yaml b/peopleCounter32Dev.yaml index 5a929d90..f61c2058 100644 --- a/peopleCounter32Dev.yaml +++ b/peopleCounter32Dev.yaml @@ -75,7 +75,7 @@ roode: zones: invert: true entry: - roi: auto + roi: auto exit: # roi: # height: 4 @@ -112,11 +112,13 @@ sensor: zones: entry: distance: + id: entryDist name: $friendly_name Entry Distance filters: - delta: 100 exit: distance: + id: exitDist name: $friendly_name Exit Distance filters: - delta: 100