|
|
|
@ -1873,6 +1873,92 @@ void GCS_MAVLINK::handle_vision_position_delta(mavlink_message_t *msg)
@@ -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)
@@ -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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|