Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow: Add separate scale factors for X and Y axis. 

The resolution is also increased to take advantage of the planned introduction of automated calibration methods
This commit is contained in:
priseborough 2014-11-11 20:04:14 +11:00 committed by Andrew Tridgell
parent 52c7e56a4a
commit 063fb41748
3 changed files with 19 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
View File

@ -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 {

18
libraries/AP_OpticalFlow/OpticalFlow.cpp
View File

@ -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
};

3
libraries/AP_OpticalFlow/OpticalFlow.h
View File

@ -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