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

msg/vehicle_odometry.msg: simplify covariance handling and update all usage (#19966)

Browse Source 
- replace float32[21] URT covariances with smaller dedicated position/velocity/orientation variances (the crossterms are unused, awkward, and relatively costly)
 - these are easier to casually inspect and more representative of what's actually being used currently and reduces the size of vehicle_odometry_s quite a bit
 - ekf2: add new helper to get roll/pitch/yaw covariances
 - mavlink: receiver ODOMETRY handle more frame types for both pose (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU) and velocity (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU, MAV_FRAME_BODY_FRD)
 - mavlink: delete unused ATT_POS_MOCAP stream (this is just a passthrough)

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2022-08-04 12:55:21 -04:00
committed by GitHub
parent 61f390b0dd
commit dfdfbbfa9c
21 changed files with 838 additions and 646 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink
+1 -1
Submodule src/modules/mavlink/mavlink updated: 05864e218e...c46af52326
src/modules/mavlink/mavlink_messages.cpp
-4
View File
@@ -120,7 +120,6 @@
#if !defined(CONSTRAINED_FLASH)
# include "streams/ADSB_VEHICLE.hpp"
# include "streams/ATT_POS_MOCAP.hpp"
# include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp"
# include "streams/DEBUG.hpp"
# include "streams/DEBUG_FLOAT_ARRAY.hpp"
@@ -401,9 +400,6 @@ static const StreamListItem streams_list[] = {
#if defined(VIBRATION_HPP)
create_stream_list_item<MavlinkStreamVibration>(),
#endif // VIBRATION_HPP
#if defined(ATT_POS_MOCAP_HPP)
create_stream_list_item<MavlinkStreamAttPosMocap>(),
#endif // ATT_POS_MOCAP_HPP
#if defined(AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP)
create_stream_list_item<MavlinkStreamAutopilotStateForGimbalDevice>(),
#endif // AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP
src/modules/mavlink/mavlink_receiver.cpp
+258 -127
View File
@@ -80,6 +80,22 @@ MavlinkReceiver::~MavlinkReceiver()
#endif // !CONSTRAINED_FLASH
}
static constexpr vehicle_odometry_s vehicle_odometry_empty {
.timestamp = 0,
.timestamp_sample = 0,
.position = {NAN, NAN, NAN},
.q = {NAN, NAN, NAN, NAN},
.velocity = {NAN, NAN, NAN},
.angular_velocity = {NAN, NAN, NAN},
.position_variance = {NAN, NAN, NAN},
.orientation_variance = {NAN, NAN, NAN},
.velocity_variance = {NAN, NAN, NAN},
.pose_frame = vehicle_odometry_s::POSE_FRAME_UNKNOWN,
.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_UNKNOWN,
.reset_counter = 0,
.quality = 0
};
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
ModuleParams(nullptr),
_mavlink(parent),
@@ -948,40 +964,39 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
void
MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
{
mavlink_att_pos_mocap_t mocap;
mavlink_msg_att_pos_mocap_decode(msg, &mocap);
mavlink_att_pos_mocap_t att_pos_mocap;
mavlink_msg_att_pos_mocap_decode(msg, &att_pos_mocap);
vehicle_odometry_s mocap_odom{};
// fill vehicle_odometry from Mavlink ATT_POS_MOCAP
vehicle_odometry_s odom{vehicle_odometry_empty};
mocap_odom.timestamp = hrt_absolute_time();
mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec);
odom.timestamp_sample = _mavlink_timesync.sync_stamp(att_pos_mocap.time_usec);
mocap_odom.x = mocap.x;
mocap_odom.y = mocap.y;
mocap_odom.z = mocap.z;
mocap_odom.q[0] = mocap.q[0];
mocap_odom.q[1] = mocap.q[1];
mocap_odom.q[2] = mocap.q[2];
mocap_odom.q[3] = mocap.q[3];
odom.q[0] = att_pos_mocap.q[0];
odom.q[1] = att_pos_mocap.q[1];
odom.q[2] = att_pos_mocap.q[2];
odom.q[3] = att_pos_mocap.q[3];
const size_t URT_SIZE = sizeof(mocap_odom.pose_covariance) / sizeof(mocap_odom.pose_covariance[0]);
static_assert(URT_SIZE == (sizeof(mocap.covariance) / sizeof(mocap.covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = att_pos_mocap.x;
odom.position[1] = att_pos_mocap.y;
odom.position[2] = att_pos_mocap.z;
for (size_t i = 0; i < URT_SIZE; i++) {
mocap_odom.pose_covariance[i] = mocap.covariance[i];
}
// ATT_POS_MOCAP covariance
// Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
// If unknown, assign NaN value to first element in the array.
odom.position_variance[0] = att_pos_mocap.covariance[0]; // X row 0, col 0
odom.position_variance[1] = att_pos_mocap.covariance[6]; // Y row 1, col 1
odom.position_variance[2] = att_pos_mocap.covariance[11]; // Z row 2, col 2
mocap_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
mocap_odom.vx = NAN;
mocap_odom.vy = NAN;
mocap_odom.vz = NAN;
mocap_odom.rollspeed = NAN;
mocap_odom.pitchspeed = NAN;
mocap_odom.yawspeed = NAN;
mocap_odom.velocity_covariance[0] = NAN;
odom.orientation_variance[0] = att_pos_mocap.covariance[15]; // R row 3, col 3
odom.orientation_variance[1] = att_pos_mocap.covariance[18]; // P row 4, col 4
odom.orientation_variance[2] = att_pos_mocap.covariance[20]; // Y row 5, col 5
_mocap_odometry_pub.publish(mocap_odom);
odom.timestamp = hrt_absolute_time();
_mocap_odometry_pub.publish(odom);
}
void
@@ -1313,143 +1328,259 @@ MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg)
void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{
mavlink_vision_position_estimate_t ev;
mavlink_msg_vision_position_estimate_decode(msg, &ev);
mavlink_vision_position_estimate_t vpe;
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
vehicle_odometry_s visual_odom{};
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
vehicle_odometry_s odom{vehicle_odometry_empty};
visual_odom.timestamp = hrt_absolute_time();
visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec);
odom.timestamp_sample = _mavlink_timesync.sync_stamp(vpe.usec);
visual_odom.x = ev.x;
visual_odom.y = ev.y;
visual_odom.z = ev.z;
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(visual_odom.q);
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = vpe.x;
odom.position[1] = vpe.y;
odom.position[2] = vpe.z;
visual_odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
q.copyTo(odom.q);
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");
// VISION_POSITION_ESTIMATE covariance
// Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
// If unknown, assign NaN value to first element in the array.
odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0
odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1
odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2
for (size_t i = 0; i < URT_SIZE; i++) {
visual_odom.pose_covariance[i] = ev.covariance[i];
}
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
visual_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
visual_odom.vx = NAN;
visual_odom.vy = NAN;
visual_odom.vz = NAN;
visual_odom.rollspeed = NAN;
visual_odom.pitchspeed = NAN;
visual_odom.yawspeed = NAN;
visual_odom.velocity_covariance[0] = NAN;
odom.reset_counter = vpe.reset_counter;
visual_odom.reset_counter = ev.reset_counter;
odom.timestamp = hrt_absolute_time();
_visual_odometry_pub.publish(visual_odom);
_visual_odometry_pub.publish(odom);
}
void
MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
{
mavlink_odometry_t odom;
mavlink_msg_odometry_decode(msg, &odom);
mavlink_odometry_t odom_in;
mavlink_msg_odometry_decode(msg, &odom_in);
vehicle_odometry_s odometry{};
// fill vehicle_odometry from Mavlink ODOMETRY
vehicle_odometry_s odom{vehicle_odometry_empty};
odometry.timestamp = hrt_absolute_time();
odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec);
odom.timestamp_sample = _mavlink_timesync.sync_stamp(odom_in.time_usec);
/* The position is in a local FRD frame */
odometry.x = odom.x;
odometry.y = odom.y;
odometry.z = odom.z;
// position x/y/z (m)
if (PX4_ISFINITE(odom_in.x) && PX4_ISFINITE(odom_in.y) && PX4_ISFINITE(odom_in.z)) {
// frame_id: Coordinate frame of reference for the pose data.
switch (odom_in.frame_id) {
case MAV_FRAME_LOCAL_NED:
// NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = odom_in.x;
odom.position[1] = odom_in.y;
odom.position[2] = odom_in.z;
break;
/**
* The quaternion of the ODOMETRY msg represents a rotation from body frame
* to a local frame
*/
matrix::Quatf q_body_to_local(odom.q);
q_body_to_local.normalize();
q_body_to_local.copyTo(odometry.q);
case MAV_FRAME_LOCAL_ENU:
// ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = odom_in.y; // y: North
odom.position[1] = odom_in.x; // x: East
odom.position[2] = -odom_in.z; // z: Up
break;
// 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");
case MAV_FRAME_LOCAL_FRD:
// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth.
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD;
odom.position[0] = odom_in.x;
odom.position[1] = odom_in.y;
odom.position[2] = odom_in.z;
break;
// 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");
case MAV_FRAME_LOCAL_FLU:
// FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth.
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD;
odom.position[0] = odom_in.x; // x: Forward
odom.position[1] = -odom_in.y; // y: Left
odom.position[2] = -odom_in.z; // z: Up
break;
// TODO: create a method to simplify covariance copy
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odometry.pose_covariance[i] = odom.pose_covariance[i];
default:
break;
}
// pose_covariance
// Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw)
// first six entries are the first ROW, next five entries are the second ROW, etc.
if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) {
switch (odom_in.frame_id) {
case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_FRD:
case MAV_FRAME_LOCAL_FLU:
// position variances copied directly
odom.position_variance[0] = odom_in.pose_covariance[0]; // X row 0, col 0
odom.position_variance[1] = odom_in.pose_covariance[6]; // Y row 1, col 1
odom.position_variance[2] = odom_in.pose_covariance[11]; // Z row 2, col 2
break;
case MAV_FRAME_LOCAL_ENU:
// ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
odom.position_variance[0] = odom_in.pose_covariance[6]; // Y row 1, col 1
odom.position_variance[1] = odom_in.pose_covariance[0]; // X row 0, col 0
odom.position_variance[2] = odom_in.pose_covariance[11]; // Z row 2, col 2
break;
default:
break;
}
}
}
/**
* PX4 expects the body's linear velocity in the local frame,
* the linear velocity is rotated from the odom child_frame to the
* local NED frame. The angular velocity needs to be expressed in the
* body (fcu_frd) frame.
*/
if (odom.child_frame_id == MAV_FRAME_BODY_FRD) {
// q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame
if (PX4_ISFINITE(odom_in.q[0])
&& PX4_ISFINITE(odom_in.q[1])
&& PX4_ISFINITE(odom_in.q[2])
&& PX4_ISFINITE(odom_in.q[3])) {
odometry.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD;
odometry.vx = odom.vx;
odometry.vy = odom.vy;
odometry.vz = odom.vz;
odom.q[0] = odom_in.q[0];
odom.q[1] = odom_in.q[1];
odom.q[2] = odom_in.q[2];
odom.q[3] = odom_in.q[3];
odometry.rollspeed = odom.rollspeed;
odometry.pitchspeed = odom.pitchspeed;
odometry.yawspeed = odom.yawspeed;
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odometry.velocity_covariance[i] = odom.velocity_covariance[i];
// pose_covariance (roll, pitch, yaw)
// states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.
// TODO: fix pose_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU
if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) {
odom.orientation_variance[0] = odom_in.pose_covariance[15]; // R row 3, col 3
odom.orientation_variance[1] = odom_in.pose_covariance[18]; // P row 4, col 4
odom.orientation_variance[2] = odom_in.pose_covariance[20]; // Y row 5, col 5
}
} else {
PX4_ERR("Body frame %" PRIu8 " not supported. Unable to publish velocity", odom.child_frame_id);
}
odometry.reset_counter = odom.reset_counter;
// velocity vx/vy/vz (m/s)
if (PX4_ISFINITE(odom_in.vx) && PX4_ISFINITE(odom_in.vy) && PX4_ISFINITE(odom_in.vz)) {
// child_frame_id: Coordinate frame of reference for the velocity in free space (twist) data.
switch (odom_in.child_frame_id) {
case MAV_FRAME_LOCAL_NED:
// NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
odom.velocity[0] = odom_in.vx;
odom.velocity[1] = odom_in.vy;
odom.velocity[2] = odom_in.vz;
break;
/**
* Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD
* The supported sources of the data/tesimator type are MAV_ESTIMATOR_TYPE_VISION,
* MAV_ESTIMATOR_TYPE_VIO and MAV_ESTIMATOR_TYPE_MOCAP
*
* @note Regarding the local frames of reference, the appropriate EKF_AID_MASK
* should be set in order to match a frame aligned (NED) or not aligned (FRD)
* with true North
*/
if (odom.frame_id == MAV_FRAME_LOCAL_NED || odom.frame_id == MAV_FRAME_LOCAL_FRD) {
case MAV_FRAME_LOCAL_ENU:
// ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
odom.velocity[0] = odom_in.vy; // y: East
odom.velocity[1] = odom_in.vx; // x: North
odom.velocity[2] = -odom_in.vz; // z: Up
break;
if (odom.frame_id == MAV_FRAME_LOCAL_NED) {
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
case MAV_FRAME_LOCAL_FRD:
// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD;
odom.velocity[0] = odom_in.vx;
odom.velocity[1] = odom_in.vy;
odom.velocity[2] = odom_in.vz;
break;
} else {
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
case MAV_FRAME_LOCAL_FLU:
// FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD;
odom.velocity[0] = odom_in.vx; // x: Forward
odom.velocity[1] = -odom_in.vy; // y: Left
odom.velocity[2] = -odom_in.vz; // z: Up
break;
case MAV_FRAME_BODY_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08).
case MAV_FRAME_BODY_OFFSET_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08).
case MAV_FRAME_BODY_FRD:
// FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle.
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD;
odom.velocity[0] = odom_in.vx;
odom.velocity[1] = odom_in.vy;
odom.velocity[2] = odom_in.vz;
break;
default:
// unsupported child_frame_id
break;
}
if ((odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION)
|| (odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO)
|| (odom.estimator_type == MAV_ESTIMATOR_TYPE_UNKNOWN)) {
// accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
_visual_odometry_pub.publish(odometry);
// velocity_covariance (vx, vy, vz)
// states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.
// TODO: fix velocity_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU, MAV_FRAME_LOCAL_FLU
if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) {
switch (odom_in.child_frame_id) {
case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_FRD:
case MAV_FRAME_LOCAL_FLU:
case MAV_FRAME_BODY_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08).
case MAV_FRAME_BODY_OFFSET_NED: // DEPRECATED: Replaced by MAV_FRAME_BODY_FRD (2019-08).
case MAV_FRAME_BODY_FRD:
// velocity covariances copied directly
odom.velocity_variance[0] = odom_in.velocity_covariance[0]; // X row 0, col 0
odom.velocity_variance[1] = odom_in.velocity_covariance[6]; // Y row 1, col 1
odom.velocity_variance[2] = odom_in.velocity_covariance[11]; // Z row 2, col 2
break;
} else if (odom.estimator_type == MAV_ESTIMATOR_TYPE_MOCAP) {
_mocap_odometry_pub.publish(odometry);
case MAV_FRAME_LOCAL_ENU:
// ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
odom.velocity_variance[0] = odom_in.velocity_covariance[6]; // Y row 1, col 1
odom.velocity_variance[1] = odom_in.velocity_covariance[0]; // X row 0, col 0
odom.velocity_variance[2] = odom_in.velocity_covariance[11]; // Z row 2, col 2
break;
} else {
PX4_ERR("Estimator source %" PRIu8 " not supported. Unable to publish pose and velocity", odom.estimator_type);
default:
// unsupported child_frame_id
break;
}
}
}
} else {
PX4_ERR("Local frame %" PRIu8 " not supported. Unable to publish pose and velocity", odom.frame_id);
// Roll/Pitch/Yaw angular speed (rad/s)
if (PX4_ISFINITE(odom_in.rollspeed)
&& PX4_ISFINITE(odom_in.pitchspeed)
&& PX4_ISFINITE(odom_in.yawspeed)) {
odom.angular_velocity[0] = odom_in.rollspeed;
odom.angular_velocity[1] = odom_in.pitchspeed;
odom.angular_velocity[2] = odom_in.yawspeed;
}
odom.reset_counter = odom_in.reset_counter;
odom.quality = odom_in.quality;
switch (odom_in.estimator_type) {
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
case MAV_ESTIMATOR_TYPE_NAIVE:
case MAV_ESTIMATOR_TYPE_VISION:
case MAV_ESTIMATOR_TYPE_VIO:
odom.timestamp = hrt_absolute_time();
_visual_odometry_pub.publish(odom);
break;
case MAV_ESTIMATOR_TYPE_MOCAP:
odom.timestamp = hrt_absolute_time();
_mocap_odometry_pub.publish(odom);
break;
case MAV_ESTIMATOR_TYPE_GPS:
case MAV_ESTIMATOR_TYPE_GPS_INS:
case MAV_ESTIMATOR_TYPE_LIDAR:
case MAV_ESTIMATOR_TYPE_AUTOPILOT:
default:
mavlink_log_critical(&_mavlink_log_pub, "ODOMETRY: estimator_type %" PRIu8 " unsupported\t",
odom_in.estimator_type);
events::send<uint8_t>(events::ID("mavlink_rcv_odom_unsup_estimator_type"), events::Log::Error,
"ODOMETRY: unsupported estimator_type {1}", odom_in.estimator_type);
return;
}
}
src/modules/mavlink/streams/ATT_POS_MOCAP.hpp
-86
View File
@@ -1,86 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef ATT_POS_MOCAP_HPP
#define ATT_POS_MOCAP_HPP
#include <uORB/topics/vehicle_odometry.h>
class MavlinkStreamAttPosMocap : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttPosMocap(mavlink); }
static constexpr const char *get_name_static() { return "ATT_POS_MOCAP"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_ATT_POS_MOCAP; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _mocap_sub.advertised() ? MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _mocap_sub{ORB_ID(vehicle_mocap_odometry)};
bool send() override
{
vehicle_odometry_s mocap;
if (_mocap_sub.update(&mocap)) {
mavlink_att_pos_mocap_t msg{};
msg.time_usec = mocap.timestamp_sample;
msg.q[0] = mocap.q[0];
msg.q[1] = mocap.q[1];
msg.q[2] = mocap.q[2];
msg.q[3] = mocap.q[3];
msg.x = mocap.x;
msg.y = mocap.y;
msg.z = mocap.z;
// msg.covariance =
mavlink_msg_att_pos_mocap_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif // ATT_POS_MOCAP_HPP
src/modules/mavlink/streams/ODOMETRY.hpp
+59 -57
View File
@@ -74,93 +74,95 @@ private:
if (_mavlink->odometry_loopback_enabled()) {
odom_updated = _vodom_sub.update(&odom);
// set the frame_id according to the local frame of the data
if (odom.local_frame == vehicle_odometry_s::LOCAL_FRAME_NED) {
msg.frame_id = MAV_FRAME_LOCAL_NED;
} else {
msg.frame_id = MAV_FRAME_LOCAL_FRD;
}
// source: external vision system
msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
} else {
odom_updated = _odom_sub.update(&odom);
msg.frame_id = MAV_FRAME_LOCAL_NED;
// source: PX4 estimator
msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
}
if (odom_updated) {
msg.time_usec = odom.timestamp_sample;
msg.child_frame_id = MAV_FRAME_BODY_FRD;
// Current position
msg.x = odom.x;
msg.y = odom.y;
msg.z = odom.z;
// set the frame_id according to the local frame of the data
switch (odom.pose_frame) {
case vehicle_odometry_s::POSE_FRAME_NED:
msg.frame_id = MAV_FRAME_LOCAL_NED;
break;
case vehicle_odometry_s::POSE_FRAME_FRD:
msg.frame_id = MAV_FRAME_LOCAL_FRD;
break;
}
switch (odom.velocity_frame) {
case vehicle_odometry_s::VELOCITY_FRAME_NED:
msg.child_frame_id = MAV_FRAME_LOCAL_NED;
break;
case vehicle_odometry_s::VELOCITY_FRAME_FRD:
msg.child_frame_id = MAV_FRAME_LOCAL_FRD;
break;
case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD:
msg.child_frame_id = MAV_FRAME_BODY_FRD;
break;
}
msg.x = odom.position[0];
msg.y = odom.position[1];
msg.z = odom.position[2];
// Current orientation
msg.q[0] = odom.q[0];
msg.q[1] = odom.q[1];
msg.q[2] = odom.q[2];
msg.q[3] = odom.q[3];
switch (odom.velocity_frame) {
case vehicle_odometry_s::BODY_FRAME_FRD:
msg.vx = odom.vx;
msg.vy = odom.vy;
msg.vz = odom.vz;
break;
case vehicle_odometry_s::LOCAL_FRAME_FRD:
case vehicle_odometry_s::LOCAL_FRAME_NED:
// Body frame to local frame
const matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q));
// Rotate linear velocity from local to body frame
const matrix::Vector3f linvel_body(R_body_to_local.transpose() *
matrix::Vector3f(odom.vx, odom.vy, odom.vz));
msg.vx = linvel_body(0);
msg.vy = linvel_body(1);
msg.vz = linvel_body(2);
break;
}
msg.vx = odom.velocity[0];
msg.vy = odom.velocity[1];
msg.vz = odom.velocity[2];
// Current body rates
msg.rollspeed = odom.rollspeed;
msg.pitchspeed = odom.pitchspeed;
msg.yawspeed = odom.yawspeed;
// get the covariance matrix size
msg.rollspeed = odom.angular_velocity[0];
msg.pitchspeed = odom.angular_velocity[1];
msg.yawspeed = odom.angular_velocity[2];
// pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
// Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.)
for (auto &pc : msg.pose_covariance) {
pc = NAN;
}
msg.pose_covariance[0] = odom.position_variance[0]; // X row 0, col 0
msg.pose_covariance[6] = odom.position_variance[1]; // Y row 1, col 1
msg.pose_covariance[11] = odom.position_variance[2]; // Z row 2, col 2
msg.pose_covariance[15] = odom.orientation_variance[0]; // R row 3, col 3
msg.pose_covariance[18] = odom.orientation_variance[1]; // P row 4, col 4
msg.pose_covariance[20] = odom.orientation_variance[2]; // Y row 5, col 5
// 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
for (size_t i = 0; i < POS_URT_SIZE; i++) {
msg.pose_covariance[i] = odom.pose_covariance[i];
// Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle
// (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.)
for (auto &vc : msg.velocity_covariance) {
vc = NAN;
}
// 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.velocity_covariance[i] = odom.velocity_covariance[i];
}
msg.velocity_covariance[0] = odom.velocity_variance[0]; // X row 0, col 0
msg.velocity_covariance[6] = odom.velocity_variance[1]; // Y row 1, col 1
msg.velocity_covariance[11] = odom.velocity_variance[2]; // Z row 2, col 2
msg.reset_counter = odom.reset_counter;
// source: PX4 estimator
msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
msg.quality = odom.quality;
mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);
return true;