From df59f3311a29c4dbac818d420d0a159410170e02 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Mon, 3 Nov 2014 11:47:09 +1100
Subject: [PATCH] AP_OpticalFlow : Add gyro scale factor correction parameter

---
 libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp | 11 ++++++-----
 libraries/AP_OpticalFlow/OpticalFlow.cpp        | 14 +++++++++++---
 libraries/AP_OpticalFlow/OpticalFlow.h          |  3 ++-
 3 files changed, 19 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
index e178d78a49..8b026a3555 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp
@@ -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;
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.cpp b/libraries/AP_OpticalFlow/OpticalFlow.cpp
index 24dc1db184..a57fb7c776 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: 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
 };
diff --git a/libraries/AP_OpticalFlow/OpticalFlow.h b/libraries/AP_OpticalFlow/OpticalFlow.h
index b8d004aecc..7be30b5b23 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  _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