spdis
50 lines
1.1 KiB
C++
50 lines
1.1 KiB
C++
#include <cstdio>
|
|
#include <csignal>
|
|
#include <atomic>
|
|
#include <unistd.h>
|
|
#include "vl53l0x.h"
|
|
|
|
static std::atomic<bool> running(true);
|
|
static void on_signal(int) { running.store(false); }
|
|
|
|
int main()
|
|
{
|
|
setvbuf(stdout, NULL, _IONBF, 0);
|
|
std::signal(SIGINT, on_signal);
|
|
std::signal(SIGTERM, on_signal);
|
|
|
|
VL53L0X sensor;
|
|
if (!sensor.init()) {
|
|
fprintf(stderr, "VL53L0X init failed\n");
|
|
return 1;
|
|
}
|
|
|
|
sensor.startMeasure();
|
|
|
|
printf("lidar_test running, Ctrl+C to stop\n");
|
|
printf("%-8s %-8s %-10s\n", "cnt", "range_mm", "status");
|
|
|
|
int count = 0;
|
|
VL53L0X_RangingMeasurementData_t data;
|
|
|
|
while (running.load()) {
|
|
if (!sensor.readResult(data)) {
|
|
printf("%-8d %-8s %-10s\n", ++count, "ERR", "timeout");
|
|
sensor.startMeasure();
|
|
usleep(50000);
|
|
continue;
|
|
}
|
|
|
|
int range = data.RangeMilliMeter;
|
|
const char* st = (data.RangeStatus == 0) ? "OK" : "SIGMA";
|
|
printf("%-8d %-8d %-10s\n", ++count, range, st);
|
|
|
|
sensor.startMeasure();
|
|
usleep(30000);
|
|
}
|
|
|
|
sensor.stop();
|
|
printf("stopped\n");
|
|
return 0;
|
|
}
|