dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: DISTANCE_SENSOR: propagate signal_quality metric

This commit is contained in:
TSC21 2020-06-20 17:03:34 +01:00 committed by Nuno Marques
parent d34b5f86cc
commit b3bbc351ca
3 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/sitl_gazebo

@ -1 +1 @@
Subproject commit 846190e8a33063a62997f2ad18c9de5c5233d9f9 Subproject commit 3cb21c0c95e06940a8aa5a2c82f37630d0fb59de

6
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -716,11 +716,15 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
ds.q[1] = dist_sensor.quaternion[1]; ds.q[1] = dist_sensor.quaternion[1];
ds.q[2] = dist_sensor.quaternion[2]; ds.q[2] = dist_sensor.quaternion[2];
ds.q[3] = dist_sensor.quaternion[3]; 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.type = dist_sensor.type;
ds.id = dist_sensor.id; ds.id = dist_sensor.id;
ds.orientation = dist_sensor.orientation; 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); _distance_sensor_pub.publish(ds);
} }

6
src/modules/simulator/simulator_mavlink.cpp
View File

@ -1144,7 +1144,11 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
dist.type = dist_mavlink->type; dist.type = dist_mavlink->type;
dist.id = dist_mavlink->id; dist.id = dist_mavlink->id;
dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2 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) { switch (dist_mavlink->orientation) {
case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270: case MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270: