Branch data Line data Source code
1 : : #include "us_processor.hpp"
2 : : #include <algorithm>
3 : : #include <cmath>
4 : : #include <cstddef>
5 : : #include <cstdint>
6 : :
7 : : #define LOG_LOCAL_LEVEL ESP_LOG_DEBUG
8 : : #include "esp_log.h"
9 : :
10 : : static const char *TAG = "UsProcessor";
11 : :
12 : : // Ratio thresholds for ping validity
13 : : static constexpr float VALID_PING_RATIO = 0.7f; // >= this → OK quality
14 : : static constexpr float INVALID_PING_RATIO = 0.4f; // < this → INSUFFICIENT_SAMPLES
15 : :
16 : : // Fraction of max_dev_cm that triggers WEAK_SIGNAL (instead of OK)
17 : : static constexpr float WEAK_VARIANCE_RATIO = 0.6f;
18 : :
19 : : // Dominant cluster parameters
20 : : static constexpr float CLUSTER_DELTA_CM = 5.0f; // max spread within a cluster
21 : : static constexpr size_t CLUSTER_MIN_SIZE = 2; // minimum cluster size to be considered
22 : :
23 : 18 : Reading UsProcessor::process(const Reading *pings, uint8_t total_pings, const UsConfig &cfg)
24 : : {
25 [ + + ]: 18 : if (total_pings == 0) {
26 : 2 : return {UsResult::INSUFFICIENT_SAMPLES, 0.0f};
27 : : }
28 : :
29 : : float samples[MAX_PINGS];
30 : : uint8_t valid_count = 0;
31 : : uint8_t timeouts = 0;
32 : : uint8_t out_of_range = 0;
33 : :
34 : : // 1. Extract valid samples and count specific errors
35 [ + + ]: 114 : for (uint8_t i = 0; i < total_pings; i++) {
36 [ + + ]: 98 : if (is_success(pings[i].result)) {
37 : 58 : samples[valid_count++] = pings[i].cm;
38 : : }
39 [ + + ]: 40 : else if (pings[i].result == UsResult::TIMEOUT) {
40 : 23 : timeouts++;
41 : : }
42 [ + + ]: 17 : else if (pings[i].result == UsResult::OUT_OF_RANGE) {
43 : 9 : out_of_range++;
44 : : }
45 : : }
46 : :
47 : : // 2. Compute valid ping ratio
48 : 16 : float ratio = static_cast<float>(valid_count) / total_pings;
49 : :
50 : : // 3. Check minimum data threshold
51 [ + + ]: 16 : if (ratio < INVALID_PING_RATIO) {
52 : 5 : ESP_LOGD(TAG, "Insufficient samples: ratio=%.2f (need >= %.2f)", ratio, INVALID_PING_RATIO);
53 : :
54 : : // Refine the error based on what happened during pings
55 [ + + ]: 5 : if (out_of_range >= timeouts && out_of_range > 0) {
56 : 2 : return {UsResult::OUT_OF_RANGE, 0.0f};
57 : : }
58 [ + + ]: 3 : if (timeouts > 0) {
59 : 2 : return {UsResult::TIMEOUT, 0.0f};
60 : : }
61 : 1 : return {UsResult::INSUFFICIENT_SAMPLES, 0.0f};
62 : : }
63 : :
64 : : // 4. Check variance
65 : 11 : float std_dev = get_std_dev(samples, valid_count);
66 [ + + ]: 11 : if (std_dev > cfg.max_dev_cm) {
67 : 1 : ESP_LOGD(TAG, "High variance: std_dev=%.2f cm (limit=%.2f cm)", std_dev, cfg.max_dev_cm);
68 : 1 : return {UsResult::HIGH_VARIANCE, 0.0f};
69 : : }
70 : :
71 : : // 5. Apply filter
72 : 10 : float distance_cm;
73 [ + + ]: 10 : if (cfg.filter == Filter::MEDIAN) {
74 : 6 : distance_cm = reduce_median(samples, valid_count);
75 : : }
76 : : else {
77 : 4 : distance_cm = reduce_dominant_cluster(samples, valid_count);
78 : : }
79 : :
80 : : // 6. Determine quality based on ping ratio
81 [ + + ]: 10 : if (ratio >= VALID_PING_RATIO) {
82 : : // Good ratio — check if variance is elevated (but still within limit)
83 [ + + ]: 8 : if (std_dev > cfg.max_dev_cm * WEAK_VARIANCE_RATIO) {
84 : 2 : ESP_LOGD(TAG, "Weak signal (high ratio, elevated variance): std_dev=%.2f", std_dev);
85 : 2 : return {UsResult::WEAK_SIGNAL, distance_cm};
86 : : }
87 : 6 : return {UsResult::OK, distance_cm};
88 : : }
89 : :
90 : : // Ratio is between INVALID and VALID thresholds → WEAK_SIGNAL
91 : 2 : ESP_LOGD(TAG, "Weak signal (low ratio): ratio=%.2f", ratio);
92 : 2 : return {UsResult::WEAK_SIGNAL, distance_cm};
93 : : }
94 : :
95 : 7 : float UsProcessor::reduce_median(float *v, std::size_t n)
96 : : {
97 : 7 : std::sort(v, v + n);
98 : 7 : return v[n / 2];
99 : : }
100 : :
101 : 4 : float UsProcessor::reduce_dominant_cluster(float *v, std::size_t n)
102 : : {
103 : 4 : std::sort(v, v + n);
104 : :
105 : 4 : float best_cluster_sum = 0.0f;
106 : 4 : size_t best_cluster_size = 0;
107 : :
108 [ + + ]: 24 : for (size_t i = 0; i < n; i++) {
109 : : float current_sum = 0.0f;
110 : : size_t current_size = 0;
111 : :
112 [ + + ]: 67 : for (size_t j = i; j < n; j++) {
113 [ + + ]: 55 : if (std::abs(v[j] - v[i]) <= CLUSTER_DELTA_CM) {
114 : 47 : current_sum += v[j];
115 : 47 : current_size++;
116 : : }
117 : : else {
118 : : break; // array is sorted, no need to continue
119 : : }
120 : : }
121 : :
122 [ + + ]: 20 : if (current_size >= CLUSTER_MIN_SIZE && current_size > best_cluster_size) {
123 : 3 : best_cluster_size = current_size;
124 : 3 : best_cluster_sum = current_sum;
125 : : }
126 : : }
127 : :
128 [ + + ]: 4 : if (best_cluster_size == 0) {
129 : 1 : ESP_LOGW(TAG, "No valid cluster found, falling back to median");
130 : 1 : return reduce_median(v, n);
131 : : }
132 : :
133 : 3 : return best_cluster_sum / static_cast<float>(best_cluster_size);
134 : : }
135 : :
136 : 11 : float UsProcessor::get_std_dev(const float *samples, uint8_t count)
137 : : {
138 : 11 : float mean = 0.0f;
139 [ + + ]: 65 : for (uint8_t i = 0; i < count; i++) mean += samples[i];
140 : 11 : mean /= count;
141 : :
142 : 11 : float variance = 0.0f;
143 [ + + ]: 65 : for (uint8_t i = 0; i < count; i++) variance += std::pow(samples[i] - mean, 2);
144 : 11 : return std::sqrt(variance / count);
145 : : }
|
