Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

uavcannode: support optical_flow

Browse Source
This commit is contained in:
Daniel Agar
2021-01-13 18:36:48 -05:00
parent 47364ce8d6
commit bc8ec5d1f5
3 changed files with 26 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/CMakeLists.txt
+1 -1
View File
@@ -94,7 +94,7 @@ target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC
# generated DSDL
set(DSDLC_INPUTS "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/com" "${LIBUAVCAN_DIR}/dsdl/uavcan")
set(DSDLC_INPUTS "${PX4_SOURCE_DIR}/src/drivers/uavcan/dsdl/com" "${LIBUAVCAN_DIR}/dsdl/uavcan")
set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated")
set(DSDLC_INPUT_FILES)
src/drivers/uavcannode/UavcanNode.cpp
+20
View File
@@ -81,6 +81,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
_air_data_static_temperature_publisher(_node),
_raw_air_data_publisher(_node),
_range_sensor_measurement(_node),
_flow_measurement_publisher(_node),
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")),
_reset_timer(_node)
@@ -307,6 +308,7 @@ void UavcanNode::Run()
_sensor_baro_sub.registerCallback();
_sensor_mag_sub.registerCallback();
_optical_flow_sub.registerCallback();
_vehicle_gps_position_sub.registerCallback();
_initialized = true;
@@ -495,6 +497,24 @@ void UavcanNode::Run()
}
}
// optical_flow -> com::hex::equipment::flow::Measurement
if (_optical_flow_sub.updated()) {
optical_flow_s optical_flow;
if (_optical_flow_sub.copy(&optical_flow)) {
com::hex::equipment::flow::Measurement measurement{};
measurement.integration_interval = optical_flow.integration_timespan * 1e-6f; // us -> s
measurement.rate_gyro_integral[0] = optical_flow.gyro_x_rate_integral;
measurement.rate_gyro_integral[1] = optical_flow.gyro_y_rate_integral;
measurement.flow_integral[0] = optical_flow.pixel_flow_x_integral;
measurement.flow_integral[1] = optical_flow.pixel_flow_y_integral;
measurement.quality = optical_flow.quality;
_flow_measurement_publisher.broadcast(measurement);
}
}
perf_end(_cycle_perf);
pthread_mutex_unlock(&_node_mutex);
src/drivers/uavcannode/UavcanNode.hpp
+5
View File
@@ -65,6 +65,8 @@
#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <uavcan/equipment/range_sensor/Measurement.hpp>
#include <com/hex/equipment/flow/Measurement.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@@ -74,6 +76,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_gps_position.h>
@@ -177,6 +180,7 @@ private:
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;
uavcan::Publisher<com::hex::equipment::flow::Measurement> _flow_measurement_publisher;
hrt_abstime _last_static_temperature_publish{0};
@@ -191,6 +195,7 @@ private:
{this, ORB_ID(distance_sensor), 2},
{this, ORB_ID(distance_sensor), 3},
};
uORB::SubscriptionCallbackWorkItem _optical_flow_sub{this, ORB_ID(optical_flow)};
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)};