From 51cd64b3b26c56649941d54950dafac4670328b1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 3 Jan 2015 10:57:53 +1100
Subject: [PATCH] SITL: make velocity and rotmat available to the optical flow
 simulator

---
 libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp | 14 +++++++++++++-
 1 file changed, 13 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp b/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp
index cdb84a232c..18fc9657ab 100644
--- a/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp
+++ b/libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp
@@ -62,11 +62,23 @@ void SITL_State::_update_flow(void)
 
     float height_agl = _sitl->state.altitude - terrain_height_amsl;
 
+    // NED velocity vector in m/s
+    Vector3f velocity(_sitl->state.speedN, 
+                      _sitl->state.speedE, 
+                      _sitl->state.speedD);
+
+    // a rotation matrix following DCM conventions
+    Matrix3f rotmat;
+    rotmat.from_euler(radians(_sitl->state.rollDeg),
+                      radians(_sitl->state.pitchDeg),
+                      radians(_sitl->state.yawDeg));
+                      
+
     state.device_id = 1;
     state.surface_quality = 0;
 
     // rubbish calculation for Paul to fill in
-    state.flowRate = Vector2f(gyro.x, gyro.y) * height_agl;
+    state.flowRate = Vector2f(gyro.x, gyro.y) * height_agl * velocity.length();
     state.bodyRate = Vector2f(gyro.x, gyro.y) * height_agl;
 
     _optical_flow->setHIL(state);