From 3530d5b348b053c7c9a077ab350233dce3cfa8d7 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 3 Apr 2020 13:35:09 +0900
Subject: [PATCH] AP_VisualOdom: MAV driver implements
 handle_vision_position_estimate

---
 libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 24 +++++++++++++++++++
 libraries/AP_VisualOdom/AP_VisualOdom_MAV.h   |  3 +++
 2 files changed, 27 insertions(+)

diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
index c752567be9..830cda575f 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
@@ -57,3 +57,27 @@ void AP_VisualOdom_MAV::handle_vision_position_delta_msg(const mavlink_message_t
                                   position_delta,
                                   packet.confidence);
 }
+
+// consume vision position estimate data and send to EKF. distances in meters
+void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude)
+{
+    Vector3f pos{x, y, z};
+
+    // send attitude and position to EKF
+    const float posErr = 0; // parameter required?
+    const float angErr = 0; // parameter required?
+    const uint32_t reset_timestamp_ms = 0; // no data available
+    AP::ahrs().writeExtNavData(_frontend.get_pos_offset(), pos, attitude, posErr, angErr, time_ms, reset_timestamp_ms);
+
+    // calculate euler orientation for logging
+    float roll;
+    float pitch;
+    float yaw;
+    attitude.to_euler(roll, pitch, yaw);
+
+    // log sensor data
+    AP::logger().Write_VisualPosition(remote_time_us, time_ms, x, y, z, degrees(roll), degrees(pitch), degrees(yaw));
+
+    // record time for health monitoring
+    _last_update_ms = AP_HAL::millis();
+}
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
index 8aa039cc91..c3bf613200 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
@@ -11,4 +11,7 @@ public:
 
     // consume vision_position_delta mavlink messages
     void handle_vision_position_delta_msg(const mavlink_message_t &msg) override;
+
+    // consume vision position estimate data and send to EKF. distances in meters
+    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude) override;
 };