From 11a14c2c3d6ffeff5a896496c0d673845b734d86 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 23:16:09 +0530 Subject: Add rotation switching to flow from mavlink --- src/drivers/px4flow/px4flow.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 99a9cc02a..9895f780a 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -546,7 +546,7 @@ PX4FLOW::collect() report.sensor_id = 0; /* rotate measurements to flight controller frame */ - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); // XXX Check this if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe..f91ae3c37 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -68,6 +68,8 @@ #include +#include + #include #include #include @@ -357,6 +359,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) /* optical flow */ mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); + + enum Rotation flow_rot; + param_get(param_find("SENS_FLOW_ROT"),&flow_rot); struct optical_flow_s f; memset(&f, 0, sizeof(f)); @@ -374,6 +379,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, f.ground_distance_m); // XXX Check this + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); -- cgit v1.2.3