GCS_MAVLink: feed vision position data into AHRS

libraries/GCS_MAVLink/GCS.h

@@ -433,6 +433,17 @@ class GCS_MAVLINK
     virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) = 0;
     void handle_common_mission_message(mavlink_message_t *msg);
 
+    void handle_vicon_position_estimate(mavlink_message_t *msg);
+    void handle_vision_position_estimate(mavlink_message_t *msg);
+    void handle_global_vision_position_estimate(mavlink_message_t *msg);
+    void handle_att_pos_mocap(mavlink_message_t *msg);
+    void _handle_common_vision_position_estimate_data(const uint64_t usec,
+                                                      const float x,
+                                                      const float y,
+                                                      const float z,
+                                                      const float roll,
+                                                      const float pitch,
+                                                      const float yaw);
     void push_deferred_messages();
 
     void lock_channel(mavlink_channel_t chan, bool lock);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1873,6 +1873,92 @@ void GCS_MAVLINK::handle_vision_position_delta(mavlink_message_t *msg)
     visual_odom->handle_msg(msg);
 }
 
+void GCS_MAVLINK::handle_vision_position_estimate(mavlink_message_t *msg)
+{
+    mavlink_vision_position_estimate_t m;
+    mavlink_msg_vision_position_estimate_decode(msg, &m);
+
+    _handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw);
+}
+
+void GCS_MAVLINK::handle_global_vision_position_estimate(mavlink_message_t *msg)
+{
+    mavlink_global_vision_position_estimate_t m;
+    mavlink_msg_global_vision_position_estimate_decode(msg, &m);
+
+    _handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw);
+}
+
+void GCS_MAVLINK::handle_vicon_position_estimate(mavlink_message_t *msg)
+{
+    mavlink_vicon_position_estimate_t m;
+    mavlink_msg_vicon_position_estimate_decode(msg, &m);
+
+    _handle_common_vision_position_estimate_data(m.usec, m.x, m.y, m.z, m.roll, m.pitch, m.yaw);
+}
+
+// there are several messages which all have identical fields in them.
+// This function provides common handling for the data contained in
+// these packets
+void GCS_MAVLINK::_handle_common_vision_position_estimate_data(const uint64_t usec,
+                                                               const float x,
+                                                               const float y,
+                                                               const float z,
+                                                               const float roll,
+                                                               const float pitch,
+                                                               const float yaw)
+{
+    // sensor assumed to be at 0,0,0 body-frame; need parameters for this?
+    // or a new message 
+    const Vector3f sensor_offset = {};
+    const Vector3f pos = {
+        x,
+        y,
+        z
+    };
+    Quaternion attitude;
+    attitude.from_euler(roll, pitch, yaw); // from_vector312?
+    const float posErr = 0; // parameter required?
+    const float angErr = 0; // parameter required?
+    const uint32_t timestamp_ms = usec * 0.001;
+    const uint32_t reset_timestamp_ms = 0; // no data available
+
+    AP::ahrs().writeExtNavData(sensor_offset,
+                               pos,
+                               attitude,
+                               posErr,
+                               angErr,
+                               timestamp_ms,
+                               reset_timestamp_ms);
+}
+
+void GCS_MAVLINK::handle_att_pos_mocap(mavlink_message_t *msg)
+{
+    mavlink_att_pos_mocap_t m;
+    mavlink_msg_att_pos_mocap_decode(msg, &m);
+
+    // sensor assumed to be at 0,0,0 body-frame; need parameters for this?
+    const Vector3f sensor_offset = {};
+    const Vector3f pos = {
+        m.x,
+        m.y,
+        m.z
+    };
+    Quaternion attitude = Quaternion(m.q);
+    const float posErr = 0; // parameter required?
+    const float angErr = 0; // parameter required?
+    const uint32_t timestamp_ms = m.time_usec * 0.001;
+    const uint32_t reset_timestamp_ms = 0; // no data available
+
+    AP::ahrs().writeExtNavData(sensor_offset,
+                               pos,
+                               attitude,
+                               posErr,
+                               angErr,
+                               timestamp_ms,
+                               reset_timestamp_ms);
+}
+
 /*
   handle messages which don't require vehicle specific data
  */
@@ -1995,6 +2081,22 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
     case MAVLINK_MSG_ID_VISION_POSITION_DELTA:
         handle_vision_position_delta(msg);
         break;
+
+    case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
+        handle_vision_position_estimate(msg);
+        break;
+
+    case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
+        handle_vision_position_estimate(msg);
+        break;
+
+    case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
+        handle_vicon_position_estimate(msg);
+        break;
+
+    case MAVLINK_MSG_ID_ATT_POS_MOCAP:
+        handle_att_pos_mocap(msg);
+        break;
     }
 
 }