fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 06:30:35 +08:00
Code Issues 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/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);