diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index dd08e4b589..122384eacf 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -11,10 +11,11 @@ float32 variance # Measurement variance (in m^2), 0 for unknown / invalid readi int8 signal_quality # Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. 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 MAV_DISTANCE_SENSOR_RADAR = 3 +uint8 TYPE_UNKNOWN = 0 +uint8 TYPE_LASER = 1 +uint8 TYPE_ULTRASOUND = 2 +uint8 TYPE_INFRARED = 3 +uint8 TYPE_RADAR = 4 float32 h_fov # Sensor horizontal field of view (rad) float32 v_fov # Sensor vertical field of view (rad) diff --git a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 8887408ff3..e742e9cc25 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp @@ -113,7 +113,7 @@ CM8JL65::CM8JL65(const char *port, uint8_t rotation) : _px4_rangefinder.set_max_distance(7.9f); // Datasheet: 8.0m _px4_rangefinder.set_fov(0.0488692f); _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_CM8JL65); - _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_LASER); } CM8JL65::~CM8JL65() diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index 3144237ab8..141b69ce1e 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/src/drivers/distance_sensor/mappydot/MappyDot.cpp @@ -272,7 +272,7 @@ MappyDot::collect() report.orientation = _sensor_rotations[index]; report.signal_quality = -1; report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + report.type = distance_sensor_s::TYPE_LASER; report.variance = 0; int instance_id; diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index da8d5321b0..8189d1bc6f 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -227,7 +227,7 @@ MB12XX::collect() report.orientation = _sensor_rotations[_sensor_index]; report.signal_quality = -1; report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.type = distance_sensor_s::TYPE_ULTRASOUND; report.variance = 0.0f; int instance_id; diff --git a/src/drivers/distance_sensor/pga460/pga460.cpp b/src/drivers/distance_sensor/pga460/pga460.cpp index 1765cde777..045ff7aaa8 100644 --- a/src/drivers/distance_sensor/pga460/pga460.cpp +++ b/src/drivers/distance_sensor/pga460/pga460.cpp @@ -773,7 +773,7 @@ void PGA460::uORB_publish_results(const float object_distance) struct distance_sensor_s report = {}; report.timestamp = hrt_absolute_time(); report.device_id = _device_id.devid; - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.type = distance_sensor_s::TYPE_ULTRASOUND; report.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; report.current_distance = object_distance; report.min_distance = MIN_DETECTABLE_DISTANCE; diff --git a/src/drivers/distance_sensor/srf02/SRF02.cpp b/src/drivers/distance_sensor/srf02/SRF02.cpp index 70ef61833b..0fdee41de4 100644 --- a/src/drivers/distance_sensor/srf02/SRF02.cpp +++ b/src/drivers/distance_sensor/srf02/SRF02.cpp @@ -39,7 +39,7 @@ SRF02::SRF02(const I2CSPIDriverConfig &config) : _px4_rangefinder(get_device_id(), config.rotation) { _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_SRF02); - _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_ULTRASOUND); _px4_rangefinder.set_max_distance(SRF02_MAX_DISTANCE); _px4_rangefinder.set_min_distance(SRF02_MIN_DISTANCE); } diff --git a/src/drivers/distance_sensor/srf05/SRF05.cpp b/src/drivers/distance_sensor/srf05/SRF05.cpp index 9612601057..f663ed7013 100644 --- a/src/drivers/distance_sensor/srf05/SRF05.cpp +++ b/src/drivers/distance_sensor/srf05/SRF05.cpp @@ -51,7 +51,7 @@ SRF05::SRF05(const uint8_t rotation) : _px4_rangefinder(0 /* no device type for GPIO input */, rotation) { _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_SRF05); - _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_ULTRASOUND); _px4_rangefinder.set_min_distance(HXSRX0X_MIN_DISTANCE); _px4_rangefinder.set_max_distance(HXSRX0X_MAX_DISTANCE); _px4_rangefinder.set_fov(0.261799); // 15 degree FOV diff --git a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp index 8c5e9b3d0c..b1cc8c3b02 100644 --- a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp +++ b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp @@ -79,7 +79,7 @@ TERARANGER::TERARANGER(const I2CSPIDriverConfig &config) : _px4_rangefinder(get_device_id(), config.rotation) { _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_TERARANGER); - _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_LASER); } TERARANGER::~TERARANGER() diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index 150d1c486d..5d58e0b7e8 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -57,7 +57,7 @@ TFMINI::TFMINI(const char *port, uint8_t rotation) : } _px4_rangefinder.set_device_id(device_id.devid); - _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_LASER); } TFMINI::~TFMINI() diff --git a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp index 6558aef348..69a544813d 100644 --- a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp +++ b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp @@ -56,7 +56,7 @@ AerotennaULanding::AerotennaULanding(const char *port, uint8_t rotation) : _px4_rangefinder.set_device_id(device_id.devid); _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_ULANDING); - _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::TYPE_RADAR); _px4_rangefinder.set_min_distance(ULANDING_MIN_DISTANCE); _px4_rangefinder.set_max_distance(ULANDING_MAX_DISTANCE); diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index c1c59ed7a9..ca80e1d2a5 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -319,7 +319,7 @@ PX4FLOW::collect() distance_report.current_distance = report.ground_distance_m; distance_report.variance = 0.0f; distance_report.signal_quality = -1; - distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + distance_report.type = distance_sensor_s::TYPE_ULTRASOUND; distance_report.device_id = device_id.devid; distance_report.orientation = _sonar_rotation; diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp index cff084292c..b01db575b5 100644 --- a/src/drivers/uavcan/sensors/rangefinder.cpp +++ b/src/drivers/uavcan/sensors/rangefinder.cpp @@ -86,17 +86,17 @@ void UavcanRangefinderBridge::range_sub_cb(const switch (msg.sensor_type) { case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR: - rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + rangefinder_type = distance_sensor_s::TYPE_ULTRASOUND; break; case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR: - rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR; + rangefinder_type = distance_sensor_s::TYPE_RADAR; break; case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR: case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED: default: - rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + rangefinder_type = distance_sensor_s::TYPE_LASER; break; } diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp index 129bd32c21..f61f9102fe 100644 --- a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp @@ -81,19 +81,19 @@ public: // sensor type switch (dist.type) { - case distance_sensor_s::MAV_DISTANCE_SENSOR_LASER: + case distance_sensor_s::TYPE_LASER: range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR; break; - case distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND: + case distance_sensor_s::TYPE_ULTRASOUND: range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR; break; - case distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR: + case distance_sensor_s::TYPE_RADAR: range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR; break; - case distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED: + case distance_sensor_s::TYPE_INFRARED: default: range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED; break; diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 9de0a208aa..93f293d23c 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -220,7 +220,7 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) message.variance = 0.1f; message.signal_quality = 100; - message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + message.type = distance_sensor_s::TYPE_LASER; message.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; message.h_fov = math::radians(50.f); message.v_fov = math::radians(30.f); diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 63937f8f59..bd44af7ac1 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -39,7 +39,7 @@ PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_or { set_device_id(device_id); set_orientation(device_orientation); - set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); // Default to type LASER + set_rangefinder_type(distance_sensor_s::TYPE_LASER); // Default to type LASER } PX4Rangefinder::~PX4Rangefinder() diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 1668db9ca8..65d40dcf2b 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -206,13 +206,13 @@ void BlockLocalPositionEstimator::Run() if (s->get().timestamp == 0) { continue; } - if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && + if (s->get().type == distance_sensor_s::TYPE_LASER && s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && _sub_lidar == nullptr) { _sub_lidar = s; mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i); - } else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && + } else if (s->get().type == distance_sensor_s::TYPE_ULTRASOUND && s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && _sub_sonar == nullptr) { _sub_sonar = s; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 84471a2287..3422891537 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -841,7 +841,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) d.min_distance = _param_ekf2_min_rng; d.max_distance = _param_ekf2_rng_a_hmax; d.current_distance = flow.distance; /* both are in m */ - d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + d.type = distance_sensor_s::TYPE_ULTRASOUND; d.device_id = device_id.devid; d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; d.variance = 0.01; @@ -887,7 +887,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) d.min_distance = 0.3f; d.max_distance = 5.0f; d.current_distance = flow.distance; /* both are in m */ - d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + d.type = distance_sensor_s::TYPE_LASER; d.device_id = device_id.devid; d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; d.variance = 0.01; @@ -939,25 +939,55 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) device_id.devid_s.address = dist_sensor.id; ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */ + ds.device_id = device_id.devid; ds.min_distance = static_cast(dist_sensor.min_distance) * 1e-2f; /* cm to m */ ds.max_distance = static_cast(dist_sensor.max_distance) * 1e-2f; /* cm to m */ ds.current_distance = static_cast(dist_sensor.current_distance) * 1e-2f; /* cm to m */ ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */ + + if (dist_sensor.signal_quality <= 0) { + // Mavlink DISTANCE_SENSOR: 0 = unknown/unset signal quality + ds.signal_quality = -1; + + } else if (dist_sensor.signal_quality == 1) { + // Mavlink DISTANCE_SENSOR: 1 = invalid signal + ds.signal_quality = 0; + + } else { + ds.signal_quality = math::constrain(dist_sensor.signal_quality, (uint8_t)1, (uint8_t)100); + } + + switch (dist_sensor.type) { + case MAV_DISTANCE_SENSOR_LASER: + ds.type = distance_sensor_s::TYPE_LASER; + break; + + case MAV_DISTANCE_SENSOR_ULTRASOUND: + ds.type = distance_sensor_s::TYPE_ULTRASOUND; + break; + + case MAV_DISTANCE_SENSOR_INFRARED: + ds.type = distance_sensor_s::TYPE_INFRARED; + break; + + case MAV_DISTANCE_SENSOR_RADAR: + ds.type = distance_sensor_s::TYPE_RADAR; + break; + + case MAV_DISTANCE_SENSOR_UNKNOWN: + default: + ds.type = distance_sensor_s::TYPE_UNKNOWN; + break; + } + ds.h_fov = dist_sensor.horizontal_fov; ds.v_fov = dist_sensor.vertical_fov; ds.q[0] = dist_sensor.quaternion[0]; ds.q[1] = dist_sensor.quaternion[1]; ds.q[2] = dist_sensor.quaternion[2]; ds.q[3] = dist_sensor.quaternion[3]; - ds.type = dist_sensor.type; - ds.device_id = device_id.devid; ds.orientation = dist_sensor.orientation; - // MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown - // quality value. Also it comes normalised between 1 and 100 while the uORB - // signal quality is normalised between 0 and 100. - ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99; - _distance_sensor_pub.publish(ds); } diff --git a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp index 9042403a48..d64f2dac61 100644 --- a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp +++ b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp @@ -63,37 +63,61 @@ private: bool updated = false; for (int i = 0; i < _distance_sensor_subs.size(); i++) { - distance_sensor_s dist_sensor; + distance_sensor_s distance_sensor; - if (_distance_sensor_subs[i].update(&dist_sensor)) { + if (_distance_sensor_subs[i].update(&distance_sensor)) { mavlink_distance_sensor_t msg{}; - msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */ + msg.time_boot_ms = distance_sensor.timestamp / 1000; // us -> ms - switch (dist_sensor.type) { - case MAV_DISTANCE_SENSOR_ULTRASOUND: + switch (distance_sensor.type) { + default: + case distance_sensor_s::TYPE_UNKNOWN: + msg.type = MAV_DISTANCE_SENSOR_UNKNOWN; + break; + + case distance_sensor_s::TYPE_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + case distance_sensor_s::TYPE_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: + case distance_sensor_s::TYPE_INFRARED: msg.type = MAV_DISTANCE_SENSOR_INFRARED; break; - default: - msg.type = MAV_DISTANCE_SENSOR_LASER; + case distance_sensor_s::TYPE_RADAR: + msg.type = MAV_DISTANCE_SENSOR_RADAR; break; } - msg.current_distance = dist_sensor.current_distance * 1e2f; // m to cm + msg.min_distance = math::constrain(roundf(distance_sensor.min_distance * 1e2f), 0.f, + static_cast(UINT16_MAX)); // m to cm + msg.max_distance = math::constrain(roundf(distance_sensor.max_distance * 1e2f), 0.f, + static_cast(UINT16_MAX)); // m to cm + msg.current_distance = math::constrain(roundf(distance_sensor.current_distance * 1e2f), 0.f, + static_cast(UINT16_MAX)); // m to cm msg.id = i; - msg.max_distance = dist_sensor.max_distance * 1e2f; // m to cm - msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm - msg.orientation = dist_sensor.orientation; - msg.covariance = dist_sensor.variance * 1e4f; // m^2 to cm^2 + msg.orientation = distance_sensor.orientation; + msg.covariance = distance_sensor.variance * 1e4f; // m^2 to cm^2 + msg.horizontal_fov = distance_sensor.h_fov; + msg.vertical_fov = distance_sensor.v_fov; + msg.quaternion[0] = distance_sensor.q[0]; + msg.quaternion[1] = distance_sensor.q[1]; + msg.quaternion[2] = distance_sensor.q[2]; + msg.quaternion[3] = distance_sensor.q[3]; + + if (distance_sensor.signal_quality < 0) { + msg.signal_quality = 0; // 0 = unknown/unset + + } else if (distance_sensor.signal_quality == 0) { + msg.signal_quality = 1; // 1 = invalid signal + + } else if (distance_sensor.signal_quality >= 0) { + msg.signal_quality = math::constrain(distance_sensor.signal_quality, (int8_t)2, (int8_t)100); + } mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 77b3e13819..7a4fb40aab 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -535,7 +535,7 @@ void Sih::send_airspeed() void Sih::send_dist_snsr() { _distance_snsr.timestamp = _now; - _distance_snsr.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + _distance_snsr.type = distance_sensor_s::TYPE_LASER; _distance_snsr.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; _distance_snsr.min_distance = _distance_snsr_min; _distance_snsr.max_distance = _distance_snsr_max;