fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 20:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcannode: add distance_sensor (all possible instances)

Browse Source
This commit is contained in:
Daniel Agar
2020-04-30 14:52:06 -04:00
committed by GitHub
parent d20bca095d
commit 3e5f85b47b
3 changed files with 47 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/UavcanNode.cpp
+36
View File
@@ -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;