Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: feed vision position data into AHRS

Browse Source
This commit is contained in:
Peter Barker 2018-03-08 11:46:06 +11:00 committed by Randy Mackay
parent c680b931dc
commit a5a36c04d1
2 changed files with 113 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/GCS_MAVLink/GCS.h
View File

@ -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);

102
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -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;
}
}