diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 138f34b8a2..52db320e49 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -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 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9bbaac5f17..b1fbb8236b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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 linvel_local(Rbl * matrix::Vector3(odom.vx, odom.vy, odom.vz)); odometry.vx = linvel_local(0); odometry.vy = linvel_local(1); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index dc52ca3dc8..b6e1aa8642 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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");