yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00: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

2
boards/px4/fmu-v4/cannode.cmake
View File

@@ -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

36
src/drivers/uavcannode/UavcanNode.cpp
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;

10
src/drivers/uavcannode/UavcanNode.hpp
View File

@@ -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 @@ private:
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)};