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

Add CANNODE_FLOW_ROT

Browse Source
This commit is contained in:
alexklimaj
2021-10-22 18:28:01 -06:00
committed by Daniel Agar
parent 5dcaadf492
commit 8088c82b6a
2 changed files with 44 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/Publishers/FlowMeasurement.hpp
+23 -5
View File
@@ -36,6 +36,7 @@
#include "UavcanPublisherBase.hpp"
#include <com/hex/equipment/flow/Measurement.hpp>
#include <conversion/rotation.h>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/optical_flow.h>
@@ -53,7 +54,16 @@ public:
UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID),
uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(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);
}
}
void PrintInfo() override
{
@@ -73,10 +83,16 @@ public:
if (uORB::SubscriptionCallbackWorkItem::update(&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;
// 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] = 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.quality = optical_flow.quality;
uavcan::Publisher<com::hex::equipment::flow::Measurement>::broadcast(measurement);
@@ -85,5 +101,7 @@ public:
uORB::SubscriptionCallbackWorkItem::registerCallback();
}
}
private:
matrix::Dcmf _rotation;
};
} // namespace uavcannode
src/drivers/uavcannode/uavcannode_params.c
+21
View File
@@ -59,3 +59,24 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000);
* @group UAVCAN
*/
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);