|
|
@ -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); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|