From 90cd46f4d7c425b207ea555b72727b11a1466a8d Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 22 Mar 2017 09:38:50 +1100
Subject: [PATCH] AP_AHRS: simulate body frame odometry sensor

---
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 29 ++++++++++++++++++++++++++--
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  1 +
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index 78b3049433..1270372c2d 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -261,10 +261,14 @@ void AP_AHRS_NavEKF::update_SITL(void)
 {
     if (_sitl == nullptr) {
         _sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
+        if (_sitl == nullptr) {
+            return;
+        }
     }
-    if (_sitl && active_EKF_type() == EKF_TYPE_SITL) {
-        const struct SITL::sitl_fdm &fdm = _sitl->state;
 
+    const struct SITL::sitl_fdm &fdm = _sitl->state;
+
+    if (active_EKF_type() == EKF_TYPE_SITL) {
         roll  = radians(fdm.rollDeg);
         pitch = radians(fdm.pitchDeg);
         yaw   = radians(fdm.yawDeg);
@@ -286,6 +290,27 @@ void AP_AHRS_NavEKF::update_SITL(void)
                                         fdm.zAccel);
         }
         _accel_ef_ekf_blended = _accel_ef_ekf[0];
+
+    }
+    // use SITL states to write body frame odometry data at 20Hz
+    uint32_t timeStamp_ms = AP_HAL::millis();
+    if (timeStamp_ms - _last_body_odm_update_ms > 50) {
+        const float quality = 100.0f;
+        const Vector3f posOffset = Vector3f(0.0f,0.0f,0.0f);
+        float delTime = 0.001f*(timeStamp_ms - _last_body_odm_update_ms);
+        _last_body_odm_update_ms = timeStamp_ms;
+        timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
+        Vector3f delAng = Vector3f(radians(fdm.rollRate),
+                                   radians(fdm.pitchRate),
+                                   radians(fdm.yawRate));
+        delAng *= delTime;
+        // rotate earth velocity into body frame and calculate delta position
+        Matrix3f Tbn;
+        Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
+        Vector3f earth_vel = Vector3f(fdm.speedN,fdm.speedE,fdm.speedD);
+        Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
+        // write to EKF
+        EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
     }
 }
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 8ecd010349..0e909194a6 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -283,6 +283,7 @@ private:
     
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     SITL::SITL *_sitl;
+    uint32_t _last_body_odm_update_ms = 0;
     void update_SITL(void);
 #endif    
 };