From a5a36c04d1056a2864267e9a84dc8edc90d9aa84 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Mar 2018 11:46:06 +1100 Subject: [PATCH] GCS_MAVLink: feed vision position data into AHRS --- libraries/GCS_MAVLink/GCS.h | 11 +++ libraries/GCS_MAVLink/GCS_Common.cpp | 102 +++++++++++++++++++++++++++ 2 files changed, 113 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index e78dfdd747..108c8dd604 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -433,6 +433,17 @@ private: 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); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 627e63ef5c..060110a5ba 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/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; } }