Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 15:08:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow : Add parameter for flow rate scale factor correction

Browse Source
This commit is contained in:
priseborough 2014-11-03 09:22:20 +11:00 committed by Andrew Tridgell
parent 2500f7e9c2
commit eec49ce1dd
3 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
View File

@ -64,8 +64,9 @@ void AP_OpticalFlow_PX4::update(void)
_device_id = report.sensor_id; _device_id = report.sensor_id;
_surface_quality = report.quality; _surface_quality = report.quality;
if (report.integration_timespan > 0) { if (report.integration_timespan > 0) {
_flowRate.x = report.pixel_flow_x_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the X sensor axis float scaleFactor = 0.01f * float(100 + _scaler);
_flowRate.y = report.pixel_flow_y_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the Y sensor axis _flowRate.x = scaleFactor * report.pixel_flow_x_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the X sensor axis
_flowRate.y = scaleFactor * report.pixel_flow_y_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the Y sensor axis
_bodyRate.x = report.gyro_x_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the X sensor axis _bodyRate.x = report.gyro_x_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the X sensor axis
_bodyRate.y = report.gyro_y_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the Y sensor axis _bodyRate.y = report.gyro_y_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the Y sensor axis
} else { } else {

8
libraries/AP_OpticalFlow/OpticalFlow.cpp
View File

@ -10,6 +10,14 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = {
// @User: Standard // @User: Standard
AP_GROUPINFO("_ENABLE", 0, OpticalFlow, _enabled, 0), AP_GROUPINFO("_ENABLE", 0, OpticalFlow, _enabled, 0),
// @Param: SCALER
// @DisplayName: Optical flow scale factor
// @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
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_SCALER", 1, OpticalFlow, _scaler, 0),
AP_GROUPEND AP_GROUPEND
}; };

1
libraries/AP_OpticalFlow/OpticalFlow.h
View File

@ -79,6 +79,7 @@ protected:
// parameters // parameters
AP_Int8 _enabled; // enabled/disabled flag AP_Int8 _enabled; // enabled/disabled flag
AP_Int8 _scaler; // flow angular rate correction percentage
// internal variables // internal variables
uint8_t _device_id; // device id uint8_t _device_id; // device id