Adaptive threshold

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;

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<uint16_t> values) {
+  auto sum = std::accumulate(values.begin(), values.end(), 0);
+  int avg = sum / values.size();
+  return avg;
+}
 }  // namespace roode
-}  // namespace esphome
+}  // namespace esphome

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<uint16_t> 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<uint16_t> samples;
   uint8_t max_samples;
   sensor::Sensor *distance_sensor{nullptr};
+  std::vector<uint16_t> idle_distances;
 };
 }  // namespace roode
 }  // namespace esphome

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

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