From dc2e0f328bee3bc7b03eab273847682b44abfc30 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 8 Apr 2020 09:33:35 +0900
Subject: [PATCH] AP_VisualOdom: fix handle_vision_position_delta_msg

position and angle deltas were swapped when passed into the EKF
time passed into the EKF was scaled as usec when it should be sec
---
 libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
index fadf0e790a..8cdcb1803a 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
@@ -47,15 +47,16 @@ void AP_VisualOdom_MAV::handle_vision_position_delta_msg(const mavlink_message_t
     _last_update_ms = now_ms;
 
     // send to EKF
+    const float time_delta_sec = packet.time_delta_usec / 1000000.0f;
     AP::ahrs_navekf().writeBodyFrameOdom(packet.confidence,
-                                         angle_delta,
                                          position_delta,
-                                         packet.time_delta_usec,
+                                         angle_delta,
+                                         time_delta_sec,
                                          now_ms,
                                          _frontend.get_pos_offset());
 
     // log sensor data
-    AP::logger().Write_VisualOdom(packet.time_delta_usec / 1000000.0f,
+    AP::logger().Write_VisualOdom(time_delta_sec,
                                   angle_delta,
                                   position_delta,
                                   packet.confidence);