From 92f5211f55c5291a2fa3a813e5a899112e98003c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 29 Dec 2016 20:17:29 +1100 Subject: [PATCH] local_position_estimator: Make optical flow data conversions consistent with uORB interface --- src/modules/local_position_estimator/sensors/flow.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 95269388d9..c500b34aca 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -86,11 +86,11 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) //warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f", //double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d)); - // compute velocities in camera frame using ground distance - // assume camera frame is body frame + // compute velocities in body frame using ground distance + // note that the integral rates in the optical_flow uORB topic are RH rotations about body axes Vector3f delta_b( + +(flow_y_rad - gyro_y_rad)*d, -(flow_x_rad - gyro_x_rad)*d, - -(flow_y_rad - gyro_y_rad)*d, 0); // rotation of flow from body to nav frame