mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 07:33:34 +08:00
msg/vehicle_odometry.msg: simplify covariance handling and update all usage (#19966)
- 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
co-authored by
Mathieu Bresciani
Mathieu Bresciani
parent
61f390b0dd
commit
dfdfbbfa9c
@@ -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
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user