uavcannode: add distance_sensor (all distances)

PX4 · Apr 30, 2020 · 868f04f · 868f04f
1 parent d20bca0
commit 868f04f
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 1 deletion.
diff --git a/boards/px4/fmu-v4/cannode.cmake b/boards/px4/fmu-v4/cannode.cmake
@@ -37,7 +37,7 @@ px4_add_board(
 		barometer/ms5611
 		bootloaders
 		#differential_pressure # all available differential pressure drivers
-		#distance_sensor # all available distance sensor drivers
+		distance_sensor # all available distance sensor drivers
 		#dshot
 		gps
 		#imu # all available imu drivers

diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp
@@ -80,6 +80,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_air_data_static_pressure_publisher(_node),
 	_air_data_static_temperature_publisher(_node),
 	_raw_air_data_publisher(_node),
+	_range_sensor_measurement(_node),
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
 	_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")),
 	_reset_timer(_node)
@@ -299,6 +300,11 @@ void UavcanNode::Run()
 		_node.setModeOperational();
 
 		_diff_pressure_sub.registerCallback();
+
+		for (auto &dist : _distance_sensor_sub) {
+			dist.registerCallback();
+		}
+
 		_sensor_baro_sub.registerCallback();
 		_sensor_mag_sub.registerCallback();
 		_vehicle_gps_position_sub.registerCallback();
@@ -373,6 +379,36 @@ void UavcanNode::Run()
 		}
 	}
 
+	// distance_sensor[] -> uavcan::equipment::range_sensor::Measurement
+	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		distance_sensor_s dist;
+
+		if (_distance_sensor_sub[i].update(&dist)) {
+			uavcan::equipment::range_sensor::Measurement range_sensor{};
+
+			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;
+
+			// reading_type
+			if (dist.current_distance >= dist.max_distance) {
+				range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR;
+
+			} else if (dist.current_distance <= dist.min_distance) {
+				range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE;
+
+			} else if (dist.signal_quality != 0) {
+				range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE;
+
+			} else {
+				range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_UNDEFINED;
+			}
+
+			_range_sensor_measurement.broadcast(range_sensor);
+		}
+	}
+
 	// sensor_baro -> uavcan::equipment::air_data::StaticTemperature
 	if (_sensor_baro_sub.updated()) {
 		sensor_baro_s baro;

diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp
@@ -63,6 +63,7 @@
 #include <uavcan/equipment/air_data/StaticTemperature.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
 #include <uavcan/equipment/power/BatteryInfo.hpp>
+#include <uavcan/equipment/range_sensor/Measurement.hpp>
 
 
 #include <lib/parameters/param.h>
@@ -73,6 +74,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -173,12 +175,20 @@ class UavcanNode : public px4::ScheduledWorkItem
 	uavcan::Publisher<uavcan::equipment::air_data::StaticPressure> _air_data_static_pressure_publisher;
 	uavcan::Publisher<uavcan::equipment::air_data::StaticTemperature> _air_data_static_temperature_publisher;
 	uavcan::Publisher<uavcan::equipment::air_data::RawAirData> _raw_air_data_publisher;
+	uavcan::Publisher<uavcan::equipment::range_sensor::Measurement> _range_sensor_measurement;
+
 	hrt_abstime _last_static_temperature_publish{0};
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 
 	uORB::SubscriptionCallbackWorkItem _battery_status_sub{this, ORB_ID(battery_status)};
 	uORB::SubscriptionCallbackWorkItem _diff_pressure_sub{this, ORB_ID(differential_pressure)};
+	uORB::SubscriptionCallbackWorkItem _distance_sensor_sub[ORB_MULTI_MAX_INSTANCES] {
+		{this, ORB_ID(distance_sensor), 0},
+		{this, ORB_ID(distance_sensor), 1},
+		{this, ORB_ID(distance_sensor), 2},
+		{this, ORB_ID(distance_sensor), 3},
+	};
 	uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)};
 	uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)};