From 8c246b3219b07065d6fc67646f0eb9cac9348817 Mon Sep 17 00:00:00 2001 From: Nicolas de Palezieux Date: Thu, 21 Apr 2016 18:17:05 +0200 Subject: [PATCH] mavlink_receiver: only publish the distance measurement of the mavlink flow message on uORB if they are valid On the snapdragon, optical flow is computed in a separate process and sent to the mainapp via mavlink. Distance measurements come from a range sensor and are already published on uORB. Here we make sure that meaningless distance values in the mavlink optical flow message do not interfere with the ones from other sources. --- src/modules/mavlink/mavlink_receiver.cpp | 28 +++++++++++++----------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index db788fabda..0724d8e3c8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -478,20 +478,22 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) struct distance_sensor_s d; memset(&d, 0, sizeof(d)); - d.timestamp = flow.integration_time_us * 1000; /* ms to us */ - d.min_distance = 0.3f; - d.max_distance = 5.0f; - d.current_distance = flow.distance; /* both are in m */ - d.type = 1; - d.id = MAV_DISTANCE_SENSOR_ULTRASOUND; - d.orientation = 8; - d.covariance = 0.0; + if(flow.distance > 0.0f) { // negative values signal invalid data + d.timestamp = flow.integration_time_us * 1000; /* ms to us */ + d.min_distance = 0.3f; + d.max_distance = 5.0f; + d.current_distance = flow.distance; /* both are in m */ + d.type = 1; + d.id = MAV_DISTANCE_SENSOR_ULTRASOUND; + d.orientation = 8; + d.covariance = 0.0; - if (_flow_distance_sensor_pub == nullptr) { - _flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); - } else { - orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d); + if (_flow_distance_sensor_pub == nullptr) { + _flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, + &_orb_class_instance, ORB_PRIO_HIGH); + } else { + orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d); + } } }