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

AP_OpticalFlow : Add gyro scale factor correction parameter

Browse Source
This commit is contained in:
priseborough 2014-11-03 11:47:09 +11:00 committed by Andrew Tridgell
parent eec49ce1dd
commit df59f3311a
3 changed files with 19 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
View File

@ -64,11 +64,12 @@ void AP_OpticalFlow_PX4::update(void)
_device_id = report.sensor_id;
_surface_quality = report.quality;
if (report.integration_timespan > 0) {
float scaleFactor = 0.01f * float(100 + _scaler);
_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.y = report.gyro_y_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the Y sensor axis
float flowScaleFactor = 0.01f * float(100 + _flowScaler);
float gyroScaleFactor = 0.01f * float(100 + _gyroScaler);
_flowRate.x = flowScaleFactor * report.pixel_flow_x_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the X sensor axis
_flowRate.y = flowScaleFactor * report.pixel_flow_y_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the Y sensor axis
_bodyRate.x = gyroScaleFactor * report.gyro_x_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the X sensor axis
_bodyRate.y = gyroScaleFactor * report.gyro_y_rate_integral / (report.integration_timespan / 1e6f); // rad/sec measured inertially about the Y sensor axis
} else {
_flowRate.x = 0.0f;
_flowRate.y = 0.0f;

14
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: SCALER
// @DisplayName: Optical flow scale factor
// @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
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_SCALER", 1, OpticalFlow, _scaler, 0),
AP_GROUPINFO("_FSCALER", 1, OpticalFlow, _flowScaler, 0),
// @Param: GSCALER
// @DisplayName: Gyro scale factor correction
// @Description: This sets the percentage scale factor correction applied to the flow sensor gyro rates. It can be used to correct for sensor errors.
// @Range: -20 +20
// @Increment: 1
// @User: Standard
AP_GROUPINFO("_GSCALER", 2, OpticalFlow, _gyroScaler, 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 _scaler; // flow angular rate correction percentage
AP_Int8 _flowScaler; // flow angular rate correction percentage
AP_Int8 _gyroScaler; // gyro angular rate correction percentage
// internal variables
uint8_t _device_id; // device id