diff --git a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp index 4cf060eebf..86bb65cd15 100644 --- a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp @@ -36,6 +36,7 @@ #include "UavcanPublisherBase.hpp" #include +#include #include #include @@ -53,7 +54,16 @@ public: UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID), uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)), uavcan::Publisher(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::broadcast(measurement); @@ -85,5 +101,7 @@ public: uORB::SubscriptionCallbackWorkItem::registerCallback(); } } +private: + matrix::Dcmf _rotation; }; } // namespace uavcannode diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 9dae5697df..f4f8b8492c 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/src/drivers/uavcannode/uavcannode_params.c @@ -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);