diff --git a/CMakeLists.txt b/CMakeLists.txt index 457a0bfa74..bfd7b662b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,6 +80,7 @@ add_message_files( vehicle_global_velocity_setpoint.msg offboard_control_mode.msg vehicle_force_setpoint.msg + distance_sensor.msg ) ## Generate services in the 'srv' folder diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 4f821b0608..75acd6a608 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -54,7 +54,7 @@ On a Debian/Ubuntu system please run: sudo apt-get install python-empy sudo pip install catkin_pkg - + On MacOS please run: sudo pip install empy catkin_pkg @@ -95,13 +95,19 @@ def convert_dir(inputdir, outputdir, templatedir): """ includepath = incl_default + [':'.join([package, inputdir])] for f in os.listdir(inputdir): + # Ignore hidden files + if f.startswith("."): + continue + fn = os.path.join(inputdir, f) - if os.path.isfile(fn): - convert_file( - fn, - outputdir, - templatedir, - includepath) + # Only look at actual files + if not os.path.isfile(fn): + continue + + convert_file(fn, + outputdir, + templatedir, + includepath) def copy_changed(inputdir, outputdir, prefix=''): diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg new file mode 100644 index 0000000000..8e44d76f79 --- /dev/null +++ b/msg/distance_sensor.msg @@ -0,0 +1,17 @@ +# DISTANCE_SENSOR message data + +uint32 time_boot_ms # Time since system boot (us) + +uint16 min_distance # Minimum distance the sensor can measure (in SI) +uint16 max_distance # Maximum distance the sensor can measure (in SI) +uint16 current_distance # Current distance reading (in SI) + +uint8 type # Type from MAV_DISTANCE_SENSOR enum +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 + +uint8 id # Onboard ID of the sensor + +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum +uint8 covariance # Measurement covariance (in SI), 0 for unknown / invalid readings \ No newline at end of file diff --git a/msg/range_finder.msg b/msg/range_finder.msg deleted file mode 100644 index d2e67543e5..0000000000 --- a/msg/range_finder.msg +++ /dev/null @@ -1,13 +0,0 @@ -int16 RANGE_FINDER_TYPE_LASER = 0 - -# range finder report structure. Reads from the device must be in multiples of this -# structure. -uint64 timestamp -uint64 error_count -uint16 type # type, following RANGE_FINDER_TYPE enum -float32 distance # in meters -float32 minimum_distance # minimum distance the sensor can measure -float32 maximum_distance # maximum distance the sensor can measure -uint8 valid # 1 == within sensor range, 0 = outside sensor range -float32[12] distance_vector # in meters length should match MB12XX_MAX_RANGEFINDERS -uint8 just_updated # number of the most recent measurement sensor diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index bd72971b94..035eb92e0c 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,16 +46,12 @@ #define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder" #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" -#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected +#define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus -#define range_finder_report range_finder_s -#define __orb_sensor_range_finder __orb_range_finder - -#include - -#ifndef RANGE_FINDER_TYPE_LASER -#define RANGE_FINDER_TYPE_LASER 0 -#endif +/* + * ObjDev tag for px4flow data. + */ +ORB_DECLARE(distance_sensor); /* * ioctl() definitions diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 6a1740cb05..432b579193 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -70,6 +70,7 @@ #include #include +#include #include @@ -131,7 +132,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; @@ -208,7 +209,7 @@ MB12XX::MB12XX(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, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), @@ -255,7 +256,7 @@ MB12XX::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ @@ -268,11 +269,11 @@ MB12XX::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 = {}; - _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) { log("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -469,8 +470,8 @@ ssize_t MB12XX::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,51 +572,20 @@ MB12XX::collect() uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ - 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); - - /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ - if (addr_ind.size() == 1) { - report.distance = si_units; - - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { - report.distance_vector[i] = 0; - } - - report.just_updated = 0; - - } else { - /* for multiple sonars connected */ - - /* don't use the orginial single sonar variable */ - report.distance = 0; - - /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ - _latest_sonar_measurements[_cycle_counter] = si_units; - - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - report.distance_vector[i] = _latest_sonar_measurements[i]; - } - - /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ - report.just_updated = _index_counter; - - /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { - report.distance_vector[addr_ind.size() + i] = 0; - } - } - - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.time_boot_ms = hrt_absolute_time(); + report.id = 1; + report.type = 1; + 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)) { @@ -840,7 +810,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -858,8 +828,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); - warnx("time: %lld", report.timestamp); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -887,13 +856,7 @@ test() } warnx("periodic read %u", i); - - /* Print the sonar rangefinder report sonar distance vector */ - for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { - warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); - } - - warnx("time: %lld", report.timestamp); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 8c11e4c14d..4c85bc8c4c 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::RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::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 58799b46ac..460f5570a8 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::RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::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 */ diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 4d5e56a961..ee6fcb3ac5 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -58,13 +58,13 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include @@ -162,7 +162,7 @@ private: struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ struct wind_estimate_s _wind; /**< wind estimate */ - struct range_finder_report _distance; /**< distance estimate */ + struct distance_sensor_s _distance; /**< distance estimate */ struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1b6e17ac37..3c3e2d2f35 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main() /* * do subscriptions */ - _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + _distance_sub = orb_subscribe(ORB_ID(distance_sensor)); _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); @@ -1471,11 +1471,11 @@ void AttitudePositionEstimatorEKF::pollData() orb_check(_distance_sub, &_newRangeData); if (_newRangeData) { - orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); - - if (_distance.valid) { - _ekf->rngMea = _distance.distance; - _distance_last_valid = _distance.timestamp; + orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance); + if ((_distance.current_distance > _distance.min_distance) + && (_distance.current_distance < _distance.max_distance)) { + _ekf->rngMea = _distance.current_distance; + _distance_last_valid = _distance.time_boot_ms; } else { _newRangeData = false; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 91a30763bb..650e9bf81a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -68,9 +68,9 @@ #include #include #include +#include #include #include -#include #include #include @@ -2178,7 +2178,6 @@ protected: } }; - class MavlinkStreamDistanceSensor : public MavlinkStream { public: @@ -2204,12 +2203,12 @@ public: unsigned get_size() { - return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_range_sub; - uint64_t _range_time; + MavlinkOrbSubscription *_distance_sensor_sub; + uint64_t _dist_sensor_time; /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); @@ -2217,36 +2216,44 @@ private: protected: explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), - _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), - _range_time(0) + _distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))), + _dist_sensor_time(0) {} void send(const hrt_abstime t) { - struct range_finder_report range; + struct distance_sensor_s dist_sensor; - if (_range_sub->update(&_range_time, &range)) { + if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) { mavlink_distance_sensor_t msg; - msg.time_boot_ms = range.timestamp / 1000; + msg.time_boot_ms = dist_sensor.time_boot_ms; - switch (range.type) { - case RANGE_FINDER_TYPE_LASER: + switch (dist_sensor.type) { + case MAV_DISTANCE_SENSOR_ULTRASOUND: + msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND; + break; + + case MAV_DISTANCE_SENSOR_LASER: msg.type = MAV_DISTANCE_SENSOR_LASER; break; + case MAV_DISTANCE_SENSOR_INFRARED: + msg.type = MAV_DISTANCE_SENSOR_INFRARED; + break; + default: msg.type = MAV_DISTANCE_SENSOR_LASER; break; } - msg.id = 0; - msg.orientation = 0; - msg.min_distance = range.minimum_distance * 100; - msg.max_distance = range.maximum_distance * 100; - msg.current_distance = range.distance * 100; - msg.covariance = 20; + msg.id = dist_sensor.id; + msg.orientation = dist_sensor.orientation; + msg.min_distance = dist_sensor.min_distance; + msg.max_distance = dist_sensor.max_distance; + msg.current_distance = dist_sensor.current_distance; + msg.covariance = dist_sensor.covariance; _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ca31ec9b6c..25d577dd00 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), - _range_pub(-1), + _distance_sensor_pub(-1), _offboard_control_mode_pub(-1), _actuator_controls_pub(-1), _global_vel_sp_pub(-1), @@ -211,6 +210,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_timesync(msg); break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + handle_message_distance_sensor(msg); + break; + default: break; } @@ -412,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) } else { orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } + + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.time_boot_ms = hrt_absolute_time(); + d.min_distance = 0.3f; + d.max_distance = 5.0f; + d.current_distance = flow.distance; + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } } void @@ -444,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } - /* Use distance value for range finder report */ - struct range_finder_report r; - memset(&r, 0, sizeof(r)); + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); - r.timestamp = hrt_absolute_time(); - r.error_count = 0; - r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.distance; - r.minimum_distance = 0.0f; - r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable - r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + d.time_boot_ms = hrt_absolute_time(); + d.min_distance = 0.3f; + d.max_distance = 5.0f; + d.current_distance = flow.distance; + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; - if (_range_pub < 0) { - _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); } else { - orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); } } @@ -497,6 +520,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) +{ + /* distance sensor */ + mavlink_distance_sensor_t dist_sensor; + mavlink_msg_distance_sensor_decode(msg, &dist_sensor); + + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.time_boot_ms = dist_sensor.time_boot_ms; + d.min_distance = dist_sensor.min_distance; + d.max_distance = dist_sensor.max_distance; + d.current_distance = dist_sensor.current_distance; + d.type = dist_sensor.type; + d.id = dist_sensor.id; + d.orientation = dist_sensor.orientation; + d.covariance = dist_sensor.covariance; + + /// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } +} + void MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fe217f3c3b..016305e796 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -73,6 +73,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -134,6 +135,7 @@ private: void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); + void handle_message_distance_sensor(mavlink_message_t *msg); void *receive_thread(void *arg); @@ -164,7 +166,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; - orb_advert_t _range_pub; + orb_advert_t _distance_sensor_pub; orb_advert_t _offboard_control_mode_pub; orb_advert_t _actuator_controls_pub; orb_advert_t _global_vel_sp_pub; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3f1047742e..5ccadde57f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -61,8 +61,6 @@ #include #include -#include - #include #include #include @@ -92,6 +90,7 @@ #include #include #include +#include #include #include #include @@ -1084,7 +1083,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; struct telemetry_status_s telemetry; - struct range_finder_report range_finder; + struct distance_sensor_s distance_sensor; struct estimator_status_report estimator_status; struct tecs_status_s tecs_status; struct system_power_s system_power; @@ -1174,7 +1173,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_vel_sp_sub; int battery_sub; int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; - int range_finder_sub; + int distance_sensor_sub; int estimator_status_sub; int tecs_status_sub; int system_power_sub; @@ -1208,7 +1207,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = -1; subs.global_vel_sp_sub = -1; subs.battery_sub = -1; - subs.range_finder_sub = -1; + subs.distance_sensor_sub = -1; subs.estimator_status_sub = -1; subs.tecs_status_sub = -1; subs.system_power_sub = -1; @@ -1820,12 +1819,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) { + /* --- DISTANCE SENSOR --- */ + if (copy_if_updated(ORB_ID(distance_sensor), &subs.distance_sensor_sub, &buf.distance_sensor)) { log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; - log_msg.body.log_DIST.bottom_rate = 0.0f; - log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); + log_msg.body.log_DIST.id = buf.distance_sensor.id; + log_msg.body.log_DIST.type = buf.distance_sensor.type; + log_msg.body.log_DIST.orientation = buf.distance_sensor.orientation; + log_msg.body.log_DIST.current_distance = buf.distance_sensor.current_distance; + log_msg.body.log_DIST.covariance = buf.distance_sensor.covariance; LOGBUFFER_WRITE_AND_COUNT(DIST); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abdf518c51..eee44c43b9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -295,12 +295,14 @@ struct log_BATT_s { float discharged; }; -/* --- DIST - DISTANCE TO SURFACE --- */ +/* --- DIST - RANGE SENSOR DISTANCE --- */ #define LOG_DIST_MSG 21 struct log_DIST_s { - float bottom; - float bottom_rate; - uint8_t flags; + uint8_t id; + uint8_t type; + uint8_t orientation; + uint16_t current_distance; + uint8_t covariance; }; /* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 11bfb00d79..7add7bf06e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -57,9 +57,6 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include ORB_DEFINE(sensor_baro, struct baro_report); -#include -ORB_DEFINE(sensor_range_finder, struct range_finder_report); - #include ORB_DEFINE(output_pwm, struct pwm_output_values); @@ -259,3 +256,6 @@ ORB_DEFINE(time_offset, struct time_offset_s); #include "topics/mc_att_ctrl_status.h" ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); + +#include "topics/distance_sensor.h" +ORB_DEFINE(distance_sensor, struct distance_sensor_s); \ No newline at end of file