zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update mavlink and sitl_gazebo to latest with odometry velocity covariance

sbg
Daniel Agar 6 years ago 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
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      Jenkinsfile
  2. 2
      Tools/sitl_gazebo
  3. 2
      mavlink/include/mavlink/v2.0
  4. 10
      src/modules/mavlink/mavlink_messages.cpp
  5. 16
      src/modules/mavlink/mavlink_receiver.cpp
  6. 7
      src/modules/simulator/simulator_mavlink.cpp

10
Jenkinsfile vendored
Unescape View File

@ -9,7 +9,7 @@ pipeline { @@ -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 { @@ -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 { @@ -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 { @@ -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 @@ @@ -1 +1 @@
Subproject commit 34d06e042c9dbd336899dbba373e32a6f19828b7
Subproject commit f8b4f545f1b2d90000e37cc53f744849d939fb52

2
mavlink/include/mavlink/v2.0

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

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

@ -2361,11 +2361,15 @@ protected: @@ -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: @@ -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
Unescape View File

@ -1254,11 +1254,14 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) @@ -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) @@ -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) @@ -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) @@ -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
Unescape View File

@ -1126,13 +1126,14 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink) @@ -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) {

Write Preview
Loading…
Cancel
Save