From ea462d80e6f0203f58b482aa53a994cdb3b9c1c0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 20 Nov 2016 08:22:02 +1100 Subject: [PATCH] AP_OpticalFlow: added _applyYaw method to backend for common handling of yaw correction --- .../AP_OpticalFlow/AP_OpticalFlow_PX4.cpp | 18 ++++++++++-------- .../AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp | 3 +++ .../AP_OpticalFlow/OpticalFlow_backend.cpp | 15 +++++++++++++++ libraries/AP_OpticalFlow/OpticalFlow_backend.h | 3 +++ 4 files changed, 31 insertions(+), 8 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp index ba2cd79247..ec3cd9b13b 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp @@ -79,18 +79,20 @@ void AP_OpticalFlow_PX4::update(void) state.device_id = report.sensor_id; state.surface_quality = report.quality; if (report.integration_timespan > 0) { - float yawAngleRad = _yawAngleRad(); - float cosYaw = cosf(yawAngleRad); - float sinYaw = sinf(yawAngleRad); const Vector2f flowScaler = _flowScaler(); float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x; float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; float integralToRate = 1e6f / float(report.integration_timespan); - // rotate sensor measurements from sensor to body frame through sensor yaw angle - state.flowRate.x = flowScaleFactorX * integralToRate * (cosYaw * float(report.pixel_flow_x_integral) - sinYaw * float(report.pixel_flow_y_integral)); // rad/sec measured optically about the X body axis - state.flowRate.y = flowScaleFactorY * integralToRate * (sinYaw * float(report.pixel_flow_x_integral) + cosYaw * float(report.pixel_flow_y_integral)); // rad/sec measured optically about the Y body axis - state.bodyRate.x = integralToRate * (cosYaw * float(report.gyro_x_rate_integral) - sinYaw * float(report.gyro_y_rate_integral)); // rad/sec measured inertially about the X body axis - state.bodyRate.y = integralToRate * (sinYaw * float(report.gyro_x_rate_integral) + cosYaw * float(report.gyro_y_rate_integral)); // rad/sec measured inertially about the Y body axis + + // rad/sec measured optically about the X body axis + state.flowRate.x = flowScaleFactorX * integralToRate * report.pixel_flow_x_integral; + state.flowRate.y = flowScaleFactorY * integralToRate * report.pixel_flow_y_integral; + _applyYaw(state.flowRate); + + // rad/sec measured inertially about the X body axis + state.bodyRate.x = integralToRate * report.gyro_x_rate_integral; + state.bodyRate.y = integralToRate * report.gyro_y_rate_integral; + _applyYaw(state.bodyRate); } else { state.flowRate.zero(); state.bodyRate.zero(); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp index 6c2f916203..98e8c46fd0 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp @@ -313,6 +313,9 @@ void AP_OpticalFlow_Pixart::update(void) integral.sum.y * flowScaleFactorY); state.flowRate *= flow_pixel_scaling / dt; + // we only apply yaw to flowRate as body rate comes from AHRS + _applyYaw(state.flowRate); + state.bodyRate = integral.gyro / dt; integral.sum.zero(); diff --git a/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp b/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp index 2c867d80f1..f5f86ec695 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow_backend.cpp @@ -36,3 +36,18 @@ void OpticalFlow_backend::_update_frontend(const struct OpticalFlow::OpticalFlow frontend._state = state; frontend._last_update_ms = AP_HAL::millis(); } + +// apply yaw angle to a vector +void OpticalFlow_backend::_applyYaw(Vector2f &v) +{ + float yawAngleRad = _yawAngleRad(); + if (is_zero(yawAngleRad)) { + return; + } + float cosYaw = cosf(yawAngleRad); + float sinYaw = sinf(yawAngleRad); + float x = v.x; + float y = v.y; + v.x = cosYaw * x - sinYaw * y; + v.y = sinYaw * x + cosYaw * y; +} diff --git a/libraries/AP_OpticalFlow/OpticalFlow_backend.h b/libraries/AP_OpticalFlow/OpticalFlow_backend.h index 02cc76fd41..e4e0609363 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow_backend.h +++ b/libraries/AP_OpticalFlow/OpticalFlow_backend.h @@ -48,6 +48,9 @@ protected: // get the yaw angle in radians float _yawAngleRad(void) const { return radians(float(frontend._yawAngle_cd) * 0.01f); } + // apply yaw angle to a vector + void _applyYaw(Vector2f &v); + // get access to AHRS object AP_AHRS_NavEKF &get_ahrs(void) { return frontend._ahrs; }