Skip to content

Commit f9af4da

Browse files
eigen-valueeigen-value
committed
all ROIs min and avgs
1 parent f1e1cdb commit f9af4da

File tree

2 files changed

+130
-33
lines changed

2 files changed

+130
-33
lines changed

‎examples/sensor_tof/sensor_tof.ino

Lines changed: 12 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -5,20 +5,14 @@
55
#include "platform_config_custom.h"
66

77
TwoWire wire(PB7, PB8);
8-
#define DEV_I2C wire
9-
10-
//#define SerialPort Serial
118

129
#define LPN_PIN PB1
1310
#define I2C_RST_PIN PB0
1411

15-
// VL53L7CX sensor_vl53l7cx(&DEV_I2C, LPN_PIN, I2C_RST_PIN);
16-
17-
unsigned long T1 = 0, T2 = 0;
18-
1912
SensorTofMatrix tof(&wire, LPN_PIN, I2C_RST_PIN);
2013

2114
void setup() {
15+
Serial.begin(9600);
2216
tof.begin();
2317
}
2418

@@ -42,7 +36,17 @@ void loop() {
4236
Serial.println(tof.get_min_range_center_right_mm());
4337
Serial.print("CENTER=");
4438
Serial.println(tof.get_min_range_center_mm());
39+
Serial.print("LEFT_AVG=");
40+
Serial.println(tof.get_avg_range_left_mm());
41+
Serial.print("RIGHT_AVG=");
42+
Serial.println(tof.get_avg_range_right_mm());
43+
Serial.print("CLEFT_AVG=");
44+
Serial.println(tof.get_avg_range_center_left_mm());
45+
Serial.print("CRIGHT_AVG=");
46+
Serial.println(tof.get_avg_range_center_right_mm());
47+
Serial.print("CENTER_AVG=");
48+
Serial.println(tof.get_avg_range_center_mm());
4549
}
4650

47-
//delay(1000);
51+
delay(1000);
4852
}

‎src/sensor_tof_matrix.h

Lines changed: 118 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -33,11 +33,14 @@ class SensorTofMatrix{
3333
int begin(){
3434
int out = 0;
3535
_wire->begin();
36+
Serial.println("wire begin done");
3637
if (_wire_boost){
3738
_wire->setClock(WIRE_BOOST_CLOCK);
3839
}
3940
out |= _sensor->begin();
41+
Serial.println("sensor begin done");
4042
out |= _sensor->init_sensor();
43+
Serial.println("sensor begin done");
4144
if (_size == 8){
4245
out |= _sensor->vl53l7cx_set_resolution(VL53L7CX_RESOLUTION_8X8);
4346
if (_ranging_freq > 0 ) {
@@ -56,7 +59,7 @@ class SensorTofMatrix{
5659
out |= _sensor->vl53l7cx_start_ranging();
5760

5861
if (_wire_boost){
59-
_wire->setClock(WIRE_BASE_CLOCK);
62+
_wire->setClock(_wire_base_clock);
6063
}
6164
return out;
6265
}
@@ -130,77 +133,167 @@ class SensorTofMatrix{
130133
return bottom_max;
131134
}
132135

133-
int get_min_range_right_mm() {
136+
int get_min_range_left_mm() {
134137
update();
135138

136-
int16_t top_min = results.distance_mm[0];
139+
int16_t _min = results.distance_mm[0];
137140

138141
for (int i=0; i < (_size==4?16:64) ;i+=_size) {
139-
top_min = min(top_min, results.distance_mm[i]);
142+
_min = min(_min, results.distance_mm[i]);
140143
if (_size==8) {
141-
top_min = min(top_min, results.distance_mm[i+1]);
144+
_min = min(_min, results.distance_mm[i+1]);
142145
}
143146
}
144147

145-
return top_min;
148+
return _min;
146149
}
147150

148-
int get_min_range_left_mm() {
151+
int get_min_range_right_mm() {
149152
update();
150153

151-
int16_t top_min = results.distance_mm[0];
154+
int16_t _min = results.distance_mm[0];
152155

153156
for (int i=(_size==4?3:6); i < (_size==4?16:64) ;i+=_size) {
154-
top_min = min(top_min, results.distance_mm[i]);
157+
_min = min(_min, results.distance_mm[i]);
155158
if (_size==8) {
156-
top_min = min(top_min, results.distance_mm[i+1]);
159+
_min = min(_min, results.distance_mm[i+1]);
157160
}
158161
}
159162

160-
return top_min;
163+
return _min;
161164
}
162165

163-
int get_min_range_center_right_mm() {
166+
int get_min_range_center_left_mm() {
164167
update();
165168

166-
int16_t top_min = results.distance_mm[0];
169+
int16_t _min = results.distance_mm[0];
167170

168171
for (int i=(_size==4?5:18); i < (_size==4?13:50) ;i+=_size) {
169-
top_min = min(top_min, results.distance_mm[i]);
172+
_min = min(_min, results.distance_mm[i]);
170173
if (_size==8) {
171-
top_min = min(top_min, results.distance_mm[i+1]);
174+
_min = min(_min, results.distance_mm[i+1]);
172175
}
173176
}
174177

175-
return top_min;
178+
return _min;
176179
}
177180

178-
int get_min_range_center_left_mm() {
181+
int get_min_range_center_right_mm() {
179182
update();
180183

181-
int16_t top_min = results.distance_mm[0];
184+
int16_t _min = results.distance_mm[0];
182185

183186
for (int i=(_size==4?6:20); i < (_size==4?14:52) ;i+=_size) {
184-
top_min = min(top_min, results.distance_mm[i]);
187+
_min = min(_min, results.distance_mm[i]);
185188
if (_size==8) {
186-
top_min = min(top_min, results.distance_mm[i+1]);
189+
_min = min(_min, results.distance_mm[i+1]);
187190
}
188191
}
189192

190-
return top_min;
193+
return _min;
191194
}
192195

193196
int get_min_range_center_mm() {
194197
update();
195198

196-
int16_t top_min = results.distance_mm[0];
199+
int16_t _min = results.distance_mm[0];
197200

198201
for (int i=(_size==5?6:19); i < (_size==4?13:51) ;i+=_size) {
199-
top_min = min(top_min, results.distance_mm[i]);
200-
top_min = min(top_min, results.distance_mm[i+1]);
202+
_min = min(_min, results.distance_mm[i]);
203+
_min = min(_min, results.distance_mm[i+1]);
201204
}
202205

203-
return top_min;
206+
return _min;
207+
}
208+
209+
// avgs
210+
211+
int get_avg_range_left_mm() {
212+
update();
213+
214+
int16_t _avg = 0;
215+
uint8_t n = 0;
216+
217+
for (int i=0; i < (_size==4?16:64) ;i+=_size) {
218+
_avg += results.distance_mm[i];
219+
n++;
220+
if (_size==8) {
221+
_avg += results.distance_mm[i+1];
222+
n++;
223+
}
224+
}
225+
226+
return _avg/n;
227+
}
228+
229+
int get_avg_range_right_mm() {
230+
update();
231+
232+
int16_t _avg = 0;
233+
uint8_t n = 0;
234+
235+
for (int i=(_size==4?3:6); i < (_size==4?16:64) ;i+=_size) {
236+
_avg += results.distance_mm[i];
237+
n++;
238+
if (_size==8) {
239+
_avg += results.distance_mm[i+1];
240+
n++;
241+
}
242+
}
243+
244+
return _avg/n;
245+
}
246+
247+
int get_avg_range_center_left_mm() {
248+
update();
249+
250+
int16_t _avg = 0;
251+
uint8_t n = 0;
252+
253+
for (int i=(_size==4?5:18); i < (_size==4?13:50) ;i+=_size) {
254+
_avg += results.distance_mm[i];
255+
n++;
256+
if (_size==8) {
257+
_avg += results.distance_mm[i+1];
258+
n++;
259+
}
260+
}
261+
262+
return _avg/n;
263+
}
264+
265+
int get_avg_range_center_right_mm() {
266+
update();
267+
268+
int16_t _avg = 0;
269+
uint8_t n = 0;
270+
271+
for (int i=(_size==4?6:20); i < (_size==4?14:52) ;i+=_size) {
272+
_avg += results.distance_mm[i];
273+
n++;
274+
if (_size==8) {
275+
_avg += results.distance_mm[i+1];
276+
n++;
277+
}
278+
}
279+
280+
return _avg/n;
281+
}
282+
283+
int get_avg_range_center_mm() {
284+
update();
285+
286+
int16_t _avg = 0;
287+
uint8_t n = 0;
288+
289+
for (int i=(_size==4?6:19); i < (_size==4?13:51) ;i+=_size) {
290+
_avg += results.distance_mm[i];
291+
n++;
292+
_avg += results.distance_mm[i+1];
293+
n++;
294+
}
295+
296+
return _avg/n;
204297
}
205298

206299
};

0 commit comments

Comments
 (0)