diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp index 643f4d39ee..133c12ae6d 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp @@ -69,14 +69,18 @@ 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); - state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis - state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis - state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis - state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis + // 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 } else { state.flowRate.zero(); state.bodyRate.zero(); diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp index 708e8ace7b..a208b14c27 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp @@ -29,6 +29,14 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("_FYSCALER", 2, OpticalFlow, _flowScalerY, 0), + // @Param: ORIENT_YAW + // @DisplayName: Flow sensor yaw alignment + // @Description: Specifies the number of centi-degrees that the flow sensor is yawed relative to the vehicle. A sensor with its X-axis pointing to the right of the vehicle X axis has a positive yaw angle. + // @Range: -18000 +18000 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("_ORIENT_YAW", 3, OpticalFlow, _yawAngle_cd, 0), + AP_GROUPEND }; diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h index 6cd7479e6b..d4a6736671 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.h +++ b/libraries/AP_OpticalFlow/OpticalFlow.h @@ -84,6 +84,7 @@ private: AP_Int8 _enabled; // enabled/disabled flag AP_Int16 _flowScalerX; // X axis flow scale factor correction - parts per thousand AP_Int16 _flowScalerY; // Y axis flow scale factor correction - parts per thousand + AP_Int16 _yawAngle_cd; // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees // state filled in by backend diff --git a/libraries/AP_OpticalFlow/OpticalFlow_backend.h b/libraries/AP_OpticalFlow/OpticalFlow_backend.h index ce5c002ddf..fadd6636f5 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow_backend.h +++ b/libraries/AP_OpticalFlow/OpticalFlow_backend.h @@ -44,6 +44,9 @@ protected: // get the flow scaling parameters Vector2f _flowScaler(void) const { return Vector2f(frontend._flowScalerX, frontend._flowScalerY); } + + // get the yaw angle in radians + float _yawAngleRad(void) const { return radians(float(frontend._yawAngle_cd) * 0.01f); } }; #endif // __OpticalFlow_backend_H__