From bfe28c1df3b9c2c139976ba5998bf3b865794cc1 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 22 Sep 2017 09:00:09 +0200 Subject: [PATCH] lpe: check for distance sensor orientation --- .../BlockLocalPositionEstimator.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 5822d25d6a..33a746102c 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -235,17 +235,17 @@ void BlockLocalPositionEstimator::update() if (s->get().timestamp == 0) { continue; } - if (s->get().type == \ - distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && + if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER && + s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && _sub_lidar == nullptr) { _sub_lidar = s; - mavlink_and_console_log_info(&mavlink_log_pub, "%sLidar detected with ID %i", msg_label, i); + mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %i", msg_label, i); - } else if (s->get().type == \ - distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && + } else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && + s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && _sub_sonar == nullptr) { _sub_sonar = s; - mavlink_and_console_log_info(&mavlink_log_pub, "%sSonar detected with ID %i", msg_label, i); + mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %i", msg_label, i); } } }