fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 09:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

add a local frame of reference field to vehicle_odometry

Browse Source
This commit is contained in:
TSC21
2018-09-14 12:09:29 +01:00
committed by Lorenz Meier
parent cc73f214d1
commit 095cdeb4b0
3 changed files with 27 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+13 -1
View File
@@ -1175,6 +1175,12 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(visual_odom.q);
// TODO:
// - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define
// a frame of reference which is not aligned with NED or ENU
// - add usage on the estimator side
visual_odom.local_frame = visual_odom.LOCAL_FRAME_NED;
const size_t URT_SIZE = sizeof(visual_odom.pose_covariance) / sizeof(visual_odom.pose_covariance[0]);
static_assert(URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
@@ -1216,6 +1222,12 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
matrix::Quatf q(odom.q);
q.copyTo(odometry.q);
// TODO:
// - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define
// a frame of reference which is not aligned with NED or ENU
// - 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]);
static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])),
@@ -1235,7 +1247,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
/* get quaternion from the msg quaternion itself and build DCM matrix from it */
Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I();
/* the linear velocities needs to be transformed to the local NED frame */
/* the linear velocities needs to be transformed to the local NED frame */\
matrix::Vector3<float> linvel_local(Rbl * matrix::Vector3<float>(odom.vx, odom.vy, odom.vz));
odometry.vx = linvel_local(0);
odometry.vy = linvel_local(1);
src/modules/simulator/simulator_mavlink.cpp
+4
View File
@@ -1157,6 +1157,8 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
q.copyTo(odom.q);
odom.local_frame = odom.LOCAL_FRAME_NED;
static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
@@ -1195,6 +1197,8 @@ int Simulator::publish_odometry_topic(mavlink_message_t *odom_mavlink)
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(odom.q);
odom.local_frame = odom.LOCAL_FRAME_NED;
static_assert(POS_URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])),
"Vision Position Estimate Pose Covariance matrix URT array size mismatch");