diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp index 3556a540a8..2871e5a89f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp @@ -64,10 +64,11 @@ void AP_OpticalFlow_PX4::update(void) _device_id = report.sensor_id; _surface_quality = report.quality; if (report.integration_timespan > 0) { - float flowScaleFactor = 0.01f * float(100 + _flowScaler); + float flowScaleFactorX = 1.0f + 0.001f * float(_flowScalerX); + float flowScaleFactorY = 1.0f + 0.001f * float(_flowScalerY); float integralToRate = 1e6f / float(report.integration_timespan); - _flowRate.x = flowScaleFactor * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis - _flowRate.y = flowScaleFactor * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis + _flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis + _flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis _bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis _bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis } else { diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp index 55867e1645..ddd97549d5 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp @@ -10,13 +10,21 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("_ENABLE", 0, OpticalFlow, _enabled, 0), - // @Param: FSCALER - // @DisplayName: Optical flow scale factor correction - // @Description: This sets the percentage scale factor correction applied to the flow sensor optical rates. It can be used to correct for variations in effective focal length. - // @Range: -20 +20 + // @Param: FXSCALER + // @DisplayName: X axis optical flow scale factor correction + // @Description: This sets the parts per thousand scale factor correction applied to the flow sensor X axis optical rate. It can be used to correct for variations in effective focal length. Each positive increment of 1 increases the scale factor applied to the X axis optical flow reading by 0.1%. Negative values reduce the scale factor. + // @Range: -200 +200 // @Increment: 1 // @User: Standard - AP_GROUPINFO("_FSCALER", 1, OpticalFlow, _flowScaler, 0), + AP_GROUPINFO("_FXSCALER", 1, OpticalFlow, _flowScalerX, 0), + + // @Param: FYSCALER + // @DisplayName: Y axis optical flow scale factor correction + // @Description: This sets the parts per thousand scale factor correction applied to the flow sensor Y axis optical rate. It can be used to correct for variations in effective focal length. Each positive increment of 1 increases the scale factor applied to the Y axis optical flow reading by 0.1%. Negative values reduce the scale factor. + // @Range: -200 +200 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("_FYSCALER", 2, OpticalFlow, _flowScalerY, 0), AP_GROUPEND }; diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h index b37a9c193f..f44465c556 100644 --- a/libraries/AP_OpticalFlow/OpticalFlow.h +++ b/libraries/AP_OpticalFlow/OpticalFlow.h @@ -79,7 +79,8 @@ protected: // parameters AP_Int8 _enabled; // enabled/disabled flag - AP_Int8 _flowScaler; // flow angular rate correction percentage + AP_Int16 _flowScalerX; // X axis flow scale factor correction - parts per thousand + AP_Int16 _flowScalerY; // Y axis flow scale factor correction - parts per thousand // internal variables uint8_t _device_id; // device id