fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 07:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

optical flow sensor pipeline overhaul

Browse Source 
- all sources of optical flow publish sensor_optical_flow
 - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow

Co-authored-by: alexklimaj <alex@arkelectron.com>
This commit is contained in:
Daniel Agar
2022-06-20 20:56:56 -04:00
co-authored by alexklimaj
parent 32544452f0
commit d5839e2dd5
49 changed files with 1415 additions and 634 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/Publishers/FlowMeasurement.hpp
+8 -19
View File
@@ -39,7 +39,7 @@
#include <conversion/rotation.h>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/vehicle_optical_flow.h>
namespace uavcannode
{
@@ -52,18 +52,9 @@ class FlowMeasurement :
public:
FlowMeasurement(px4::WorkItem *work_item, uavcan::INode &node) :
UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID),
uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)),
uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_optical_flow)),
uavcan::Publisher<com::hex::equipment::flow::Measurement>(node)
{
_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, 0.f}};
param_t rot = param_find("CANNODE_FLOW_ROT");
int32_t val = 0;
if (param_get(rot, &val) == PX4_OK) {
_rotation = get_rot_matrix((enum Rotation)val);
}
this->setPriority(uavcan::TransferPriority::Default);
}
@@ -80,20 +71,18 @@ public:
void BroadcastAnyUpdates() override
{
// optical_flow -> com::hex::equipment::flow::Measurement
optical_flow_s optical_flow;
vehicle_optical_flow_s optical_flow;
if (uORB::SubscriptionCallbackWorkItem::update(&optical_flow)) {
com::hex::equipment::flow::Measurement measurement{};
measurement.integration_interval = optical_flow.integration_timespan * 1e-6f; // us -> s
measurement.integration_interval = optical_flow.integration_timespan_us * 1e-6f; // us -> s
// rotate measurements in yaw from sensor frame to body frame
const matrix::Vector3f gyro_flow_rotated = _rotation * matrix::Vector3f{optical_flow.gyro_x_rate_integral, optical_flow.gyro_y_rate_integral, 0.f};
const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{optical_flow.pixel_flow_x_integral, optical_flow.pixel_flow_y_integral, 0.f};
measurement.rate_gyro_integral[0] = optical_flow.delta_angle[0];
measurement.rate_gyro_integral[1] = optical_flow.delta_angle[1];
measurement.rate_gyro_integral[0] = gyro_flow_rotated(0);
measurement.rate_gyro_integral[1] = gyro_flow_rotated(1);
measurement.flow_integral[0] = pixel_flow_rotated(0);
measurement.flow_integral[1] = pixel_flow_rotated(1);
measurement.flow_integral[0] = optical_flow.pixel_flow[0];
measurement.flow_integral[1] = optical_flow.pixel_flow[1];
measurement.quality = optical_flow.quality;
src/drivers/uavcannode/uavcannode_params.c
-21
View File
@@ -60,27 +60,6 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000);
*/
PARAM_DEFINE_INT32(CANNODE_TERM, 0);
/**
* Cannode flow board rotation
*
* This parameter defines the yaw rotation of the Cannode flow board relative to the vehicle body frame.
* Zero rotation is defined as X on flow board pointing towards front of vehicle.
*
* @value 0 No rotation
* @value 1 Yaw 45°
* @value 2 Yaw 90°
* @value 3 Yaw 135°
* @value 4 Yaw 180°
* @value 5 Yaw 225°
* @value 6 Yaw 270°
* @value 7 Yaw 315°
*
* @reboot_required true
*
* @group UAVCAN
*/
PARAM_DEFINE_INT32(CANNODE_FLOW_ROT, 0);
/**
* Enable RTCM pub/sub
*