From 9313535a3514b8d7edb48b88facc35023992ef36 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 17 Aug 2021 21:28:37 +0900
Subject: [PATCH] AP_OpticalFlow: SITL driver applies _FX/YSCALER params

---
 libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp
index 39fcba448c..d6208a6aae 100644
--- a/libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp
+++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp
@@ -86,12 +86,17 @@ void AP_OpticalFlow_SITL::update(void)
     // correct relative velocity for rotation rates and sensor offset
     relVelSensor += gyro % posRelSensorBF;
 
+    // scaling based on parameters
+    const Vector2f flowScaler = _flowScaler();
+    const float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
+    const float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
+
     // Divide velocity by range and add body rates to get predicted sensed angular
     // optical rates relative to X and Y sensor axes assuming no misalignment or scale
     // factor error. Note - these are instantaneous values. The sensor sums these values across the interval from the last
     // poll to provide a delta angle across the interface
-    state.flowRate.x =  -relVelSensor.y/range + gyro.x + _sitl->flow_noise * rand_float();
-    state.flowRate.y =   relVelSensor.x/range + gyro.y + _sitl->flow_noise * rand_float();
+    state.flowRate.x = (-relVelSensor.y/range + gyro.x + _sitl->flow_noise * rand_float()) * flowScaleFactorX;
+    state.flowRate.y =  (relVelSensor.x/range + gyro.y + _sitl->flow_noise * rand_float()) * flowScaleFactorY;
 
     // The flow sensors body rates are assumed to be the same as the vehicle body rates (ie no misalignment)
     // Note - these are instantaneous values. The sensor sums these values across the interval from the last