dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

add a local frame of reference field to vehicle_odometry

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
msg/vehicle_odometry.msg
View File

@ -15,7 +15,15 @@ uint8 COVARIANCE_MATRIX_ROLLRATE_VARIANCE=15
uint8 COVARIANCE_MATRIX_PITCHRATE_VARIANCE=18
uint8 COVARIANCE_MATRIX_YAWRATE_VARIANCE=20
# Position in NED earth-fixed frame (meters). NaN if invalid/unknown
# Position and linear velocity frame of reference constants
uint8 LOCAL_FRAME_NED=0 # NED earth-fixed frame
uint8 LOCAL_FRAME_ENU=1 # ENU earth-fixed frame
uint8 LOCAL_FRAME_OTHER=2 # Not aligned with the std frames of reference
# Position and linear velocity local frame of reference
uint8 local_frame
# Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown
float32 x # North position
float32 y # East position
float32 z # Down position
@ -30,7 +38,7 @@ float32[4] q # Quaternion rotation from NED earth-fixed frame to XYZ body fram
# If orientation covariance invalid/unknown, 16th cell is NaN
float32[21] pose_covariance
# Velocity in NED earth-fixed frame (meters/sec). NaN if invalid/unknown
# Velocity in meters/sec. Frame of reference defined by local_frame. NaN if invalid/unknown
float32 vx # North velocity
float32 vy # East velocity
float32 vz # Down velocity

14
src/modules/mavlink/mavlink_receiver.cpp
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);

4
src/modules/simulator/simulator_mavlink.cpp
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");