fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

update mavlink and sitl_gazebo to latest with odometry velocity covariance

Browse Source
This commit is contained in:
Daniel Agar 2019-02-10 16:32:59 -05:00 committed by GitHub
parent fafadd4686
commit 7c3999e00e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 29 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
Jenkinsfile vendored
View File

@ -9,7 +9,7 @@ pipeline {
stage('Catkin build on ROS workspace') {
agent {
docker {
image 'px4io/px4-dev-ros:2019-01-27'
image 'px4io/px4-dev-ros:2019-02-09'
args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
}
}
@ -19,7 +19,8 @@ pipeline {
echo $0;
mkdir -p catkin_ws/src;
cd catkin_ws;
git clone --recursive https://github.com/PX4/sitl_gazebo.git src/mavlink_sitl_gazebo;
git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo
git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo;
source /opt/ros/melodic/setup.bash;
catkin init;
catkin build -j$(nproc) -l$(nproc);
@ -44,7 +45,7 @@ pipeline {
stage('Colcon build on ROS2 workspace') {
agent {
docker {
image 'px4io/px4-dev-ros2-bouncy:2019-01-27'
image 'px4io/px4-dev-ros2-bouncy:2019-02-09'
args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
}
}
@ -55,7 +56,8 @@ pipeline {
unset ROS_DISTRO;
mkdir -p colcon_ws/src;
cd colcon_ws;
git clone --recursive https://github.com/PX4/sitl_gazebo.git src/mavlink_sitl_gazebo;
git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo
git clone --recursive ${WORKSPACE}/colcon_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo;
source /opt/ros/bouncy/setup.sh;
source /opt/ros/melodic/setup.sh;
colcon build --event-handlers console_direct+ --symlink-install;

2
Tools/sitl_gazebo

@ -1 +1 @@
Subproject commit 34d06e042c9dbd336899dbba373e32a6f19828b7
Subproject commit f8b4f545f1b2d90000e37cc53f744849d939fb52

2
mavlink/include/mavlink/v2.0

@ -1 +1 @@
Subproject commit cf858b4513d4eb1689b7c45a30531ea2e65b589c
Subproject commit bf68eed6f6819ca1eca44217955794af554d0369

10
src/modules/mavlink/mavlink_messages.cpp
View File

@ -2361,11 +2361,15 @@ protected:
msg.yawspeed = odom.yawspeed;
// get the covariance matrix size
// pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
static_assert(VEL_URT_SIZE == (sizeof(msg.twist_covariance) / sizeof(msg.twist_covariance[0])),
// velocity_covariance
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(VEL_URT_SIZE == (sizeof(msg.velocity_covariance) / sizeof(msg.velocity_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
// copy pose covariances
@ -2376,7 +2380,7 @@ protected:
// copy velocity covariances
//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
msg.twist_covariance[i] = odom.velocity_covariance[i];
msg.velocity_covariance[i] = odom.velocity_covariance[i];
}
mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);

16
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1254,11 +1254,14 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
// - add usage on the estimator side
odometry.local_frame = odometry.LOCAL_FRAME_NED;
const size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
const size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]);
// pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
static_assert(VEL_URT_SIZE == (sizeof(odom.twist_covariance) / sizeof(odom.twist_covariance[0])),
// velocity_covariance
static constexpr size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]);
static_assert(VEL_URT_SIZE == (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
// create a method to simplify covariance copy
@ -1285,7 +1288,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.twist_covariance[i];
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}
} else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */
@ -1307,13 +1310,14 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
//TODO: Apply rotation matrix to transform from body-fixed to earth-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.twist_covariance[i];
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}
}
} else if (odom.child_frame_id == MAV_FRAME_VISION_NED || /* WRT to vehicle local NED frame */
odom.child_frame_id == MAV_FRAME_MOCAP_NED) {
if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
@ -1332,7 +1336,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
//TODO: Apply rotation matrix to transform from earth-fixed to body-fixed NED frame
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.twist_covariance[i];
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
}
}

7
src/modules/simulator/simulator_mavlink.cpp
View File

@ -1126,13 +1126,14 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
odom.pitchspeed = odom_msg.pitchspeed;
odom.yawspeed = odom_msg.yawspeed;
const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(VEL_URT_SIZE == (sizeof(odom_msg.twist_covariance) / sizeof(odom_msg.twist_covariance[0])),
// velocity_covariance
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(VEL_URT_SIZE == (sizeof(odom_msg.velocity_covariance) / sizeof(odom_msg.velocity_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
/* The velocity covariance URT */
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odom.velocity_covariance[i] = odom_msg.twist_covariance[i];
odom.velocity_covariance[i] = odom_msg.velocity_covariance[i];
}
} else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {