diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 22cebd207c..71f4134790 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -329,6 +329,8 @@ test(const bool use_i2c, const int bus) } warnx("periodic read %u", i); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index a67db0bd1f..45ce7200ba 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -832,6 +832,7 @@ test() } warnx("single read"); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ @@ -860,6 +861,9 @@ test() } warnx("periodic read %u", i); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f", (double)report.current_distance); warnx("time: %llu", report.timestamp); } diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5b1b37f563..b331aff08b 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -854,7 +854,7 @@ test() } warnx("single read"); - warnx("val: %0.2f m", (double)report.current_distance); + warnx("measurement: %0.2f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); /* start the sensor polling at 2 Hz rate */ @@ -885,7 +885,9 @@ test() } warnx("read #%u", i); - warnx("val: %0.3f m", (double)report.current_distance); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); } diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 70455b7d77..606f37ba21 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -125,6 +125,7 @@ private: work_s _work; ringbuffer::RingBuffer *_reports; bool _sensor_ok; + uint8_t _valid; int _measure_ticks; bool _collect_phase; int _class_instance; @@ -211,7 +212,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -/* static uint8_t crc8(uint8_t *p, uint8_t len) { + static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; @@ -221,7 +222,7 @@ static const uint8_t crc_table[] = { } return crc & 0xFF; -}*/ +} /* * Driver 'main' command. @@ -234,6 +235,7 @@ TRONE::TRONE(int bus, int address) : _max_distance(TRONE_MAX_DISTANCE), _reports(nullptr), _sensor_ok(false), + _valid(0), _measure_ticks(0), _collect_phase(false), _class_instance(-1), @@ -592,6 +594,10 @@ TRONE::collect() /* TODO: set proper ID */ report.id = 0; + // This validation check can be used later + _valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0; + /* publish it, if we are the primary */ if (_distance_sensor_topic != nullptr) { orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);