From 8215b9237184e44f6a60a9fca4b6fd1ab2596ff2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 23 Dec 2016 15:02:36 +0900 Subject: [PATCH] AP_RangeFinder: MAV only accepts DISTANCE_SENSOR with orient 25 25 is MAV_SENSOR_ROTATION_PITCH_270 meaning downward facing --- libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp index b56298b33a..d16325151d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.cpp @@ -51,8 +51,11 @@ void AP_RangeFinder_MAVLink::handle_msg(mavlink_message_t *msg) mavlink_distance_sensor_t packet; mavlink_msg_distance_sensor_decode(msg, &packet); - last_update_ms = AP_HAL::millis(); - distance_cm = packet.current_distance; + // only accept distances for downward facing sensors + if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_270) { + last_update_ms = AP_HAL::millis(); + distance_cm = packet.current_distance; + } } /*