From eec49ce1dd468ff313c9edb6dface8bf3a5830aa Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Mon, 3 Nov 2014 09:22:20 +1100
Subject: [PATCH] AP_OpticalFlow : Add parameter for flow rate scale factor
 correction

---
 libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp | 5 +++--
 libraries/AP_OpticalFlow/OpticalFlow.cpp        | 8 ++++++++
 libraries/AP_OpticalFlow/OpticalFlow.h          | 1 +
 3 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
index 4d76a05b0f..e178d78a49 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
@@ -64,8 +64,9 @@ void AP_OpticalFlow_PX4::update(void)
         _device_id = report.sensor_id;
         _surface_quality = report.quality;
         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
-            _flowRate.y = report.pixel_flow_y_integral / (report.integration_timespan / 1e6f); // rad/sec measured optically about the Y sensor axis
+            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
         } else {
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp
index 6cbf92cc5f..24dc1db184 100644
--- a/libraries/AP_OpticalFlow/OpticalFlow.cpp
+++ b/libraries/AP_OpticalFlow/OpticalFlow.cpp
@@ -10,6 +10,14 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = {
     // @User: Standard
     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
 };
 
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h
index 0b81ce6a28..b8d004aecc 100644
--- a/libraries/AP_OpticalFlow/OpticalFlow.h
+++ b/libraries/AP_OpticalFlow/OpticalFlow.h
@@ -79,6 +79,7 @@ protected:
 
     // parameters
     AP_Int8  _enabled;              // enabled/disabled flag
+    AP_Int8  _scaler;               // flow angular rate correction percentage
 
     // internal variables
     uint8_t _device_id;             // device id