Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: support ODOMETRY message for VIO 

used by VOXL
This commit is contained in:
Andrew Tridgell 2021-12-18 12:36:12 +11:00
parent 05f112ea17
commit 20dd667fdc
2 changed files with 44 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/GCS.h
View File

@ -864,6 +864,7 @@ private:
void handle_vision_position_estimate(const mavlink_message_t &msg);
void handle_global_vision_position_estimate(const mavlink_message_t &msg);
void handle_att_pos_mocap(const mavlink_message_t &msg);
void handle_odometry(const mavlink_message_t &msg);
void handle_common_vision_position_estimate_data(const uint64_t usec,
const float x,
const float y,

44
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3194,6 +3194,44 @@ void GCS_MAVLINK::handle_vicon_position_estimate(const mavlink_message_t &msg)
PAYLOAD_SIZE(chan, VICON_POSITION_ESTIMATE));
}
/*
handle ODOMETRY message. This message combines position, velocity
and attitude data
*/
void GCS_MAVLINK::handle_odometry(const mavlink_message_t &msg)
{
#if HAL_VISUALODOM_ENABLED
AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom == nullptr) {
return;
}
mavlink_odometry_t m;
mavlink_msg_odometry_decode(&msg, &m);
if (m.frame_id != MAV_FRAME_LOCAL_FRD ||
m.child_frame_id != MAV_FRAME_BODY_FRD) {
// only support local FRD frame data
return;
}
Quaternion q{m.q[0],m.q[1],m.q[2],m.q[3]};
float posErr = 0;
float angErr = 0;
if (!isnan(m.pose_covariance[0])) {
posErr = cbrtf(sq(m.pose_covariance[0])+sq(m.pose_covariance[6])+sq(m.pose_covariance[11]));
angErr = cbrtf(sq(m.pose_covariance[15])+sq(m.pose_covariance[18])+sq(m.pose_covariance[20]));
}
const uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ODOMETRY));
visual_odom->handle_vision_position_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, q, posErr, angErr, m.reset_counter);
const Vector3f vel{m.vx, m.vy, m.vz};
visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, m.reset_counter);
#endif
}
// there are several messages which all have identical fields in them.
// This function provides common handling for the data contained in
// these packets
@ -3242,7 +3280,7 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
return;
}
// note: att_pos_mocap does not include reset counter
visual_odom->handle_vision_position_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q, 0);
visual_odom->handle_vision_position_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q, 0, 0, 0);
#endif
}
@ -3585,6 +3623,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
handle_vicon_position_estimate(msg);
break;
case MAVLINK_MSG_ID_ODOMETRY:
handle_odometry(msg);
break;
case MAVLINK_MSG_ID_ATT_POS_MOCAP:
handle_att_pos_mocap(msg);
break;