diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index edbb4eefe2..7e4a5e015e 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -575,7 +575,7 @@ MB12XX::collect() struct distance_sensor_s report; report.time_boot_ms = hrt_absolute_time(); - report.id = 0; + report.id = 1; report.type = 1; report.orientation = 8; report.current_distance = si_units; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index c45e73fd9a..403ac89dc1 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -69,6 +69,7 @@ #include #include +#include #include @@ -128,7 +129,7 @@ private: enum SF0X_PARSE_STATE _parse_state; hrt_abstime _last_read; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; unsigned _consecutive_fail_count; @@ -194,7 +195,7 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), @@ -270,7 +271,7 @@ SF0X::init() if (ret != OK) break; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { warnx("mem err"); ret = -1; @@ -278,11 +279,11 @@ SF0X::init() } /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; + struct distance_sensor_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { warnx("advert err"); } } while(0); @@ -442,8 +443,8 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t SF0X::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -571,18 +572,19 @@ SF0X::collect() debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); - struct range_finder_report report; + struct distance_sensor_s report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); + report.time_boot_ms = hrt_absolute_time(); + report.id = 2; + report.type = 0; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -815,7 +817,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); @@ -832,8 +834,8 @@ test() } warnx("single read"); - warnx("val: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2 Hz rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -863,8 +865,8 @@ test() } warnx("read #%u", i); - warnx("val: %0.3f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.3f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to the default rate */ diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index a3c8633725..9fde00cafa 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -68,6 +68,7 @@ #include #include +#include #include @@ -128,7 +129,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -209,7 +210,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; @@ -219,7 +220,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) { } return crc & 0xFF; -} +}*/ /* * Driver 'main' command. @@ -235,7 +236,7 @@ TRONE::TRONE(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows")) @@ -281,7 +282,7 @@ TRONE::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -291,12 +292,12 @@ TRONE::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s rf_report; measure(); _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -468,8 +469,8 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t TRONE::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -568,20 +569,20 @@ TRONE::collect() uint16_t distance = (val[0] << 8) | val[1]; float si_units = distance * 0.001f; /* mm to m */ - struct range_finder_report report; - - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + struct distance_sensor_s report; + report.time_boot_ms = hrt_absolute_time(); + report.id = 3; + report.type = 0; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -787,7 +788,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -805,8 +806,8 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -834,9 +835,8 @@ test() } warnx("periodic read %u", i); - warnx("valid %u", report.valid); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.3f", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */