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