From 328477f0be7d656b25b16bc7322700b1f9942375 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Thu, 14 May 2020 13:55:09 -0700 Subject: [PATCH] UavcanNode: Add sensor_type to range_sensor pub. --- src/drivers/uavcannode/UavcanNode.cpp | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index c929dd7224..d0a1480a9d 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -389,7 +389,26 @@ void UavcanNode::Run() range_sensor.sensor_id = i; range_sensor.range = dist.current_distance; range_sensor.field_of_view = dist.h_fov; - range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED; + + // sensor type + switch (dist.type) { + case distance_sensor_s::MAV_DISTANCE_SENSOR_LASER: + range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR; + break; + + case distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND: + range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR; + break; + + case distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR: + range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR; + break; + + case distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED: + default: + range_sensor.sensor_type = uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED; + break; + } // reading_type if (dist.current_distance >= dist.max_distance) {