From b3bbc351caefa7e4858f1dd3926db9580e92a2db Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 20 Jun 2020 17:03:34 +0100 Subject: [PATCH] mavlink: DISTANCE_SENSOR: propagate signal_quality metric --- Tools/sitl_gazebo | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 6 +++++- src/modules/simulator/simulator_mavlink.cpp | 6 +++++- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 846190e8a3..3cb21c0c95 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 846190e8a33063a62997f2ad18c9de5c5233d9f9 +Subproject commit 3cb21c0c95e06940a8aa5a2c82f37630d0fb59de diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0e9c86e8ff..c4f9f870fb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -716,11 +716,15 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) ds.q[1] = dist_sensor.quaternion[1]; ds.q[2] = dist_sensor.quaternion[2]; ds.q[3] = dist_sensor.quaternion[3]; - ds.signal_quality = -1; // TODO: A dist_sensor.signal_quality field is missing from the mavlink message definition. ds.type = dist_sensor.type; ds.id = dist_sensor.id; 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/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index fe08608626..ce2e9cefa0 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1144,7 +1144,11 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl dist.type = dist_mavlink->type; dist.id = dist_mavlink->id; dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2 - dist.signal_quality = -1; + + // 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. + dist.signal_quality = dist_mavlink->signal_quality == 0 ? -1 : 100 * (dist_mavlink->signal_quality - 1) / 99; switch (dist_mavlink->orientation) { case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270: