fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-07 10:21:36 +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
co-authored by Mathieu Bresciani
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/local_position_estimator/sensors/mocap.cpp
+8 -13
View File
@@ -62,15 +62,10 @@ void BlockLocalPositionEstimator::mocapInit()
int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
{
uint8_t x_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_X_VARIANCE;
uint8_t y_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Y_VARIANCE;
uint8_t z_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Z_VARIANCE;
if (PX4_ISFINITE(_sub_mocap_odom.get().pose_covariance[x_variance])) {
// check if the mocap data is valid based on the covariances
_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().pose_covariance[x_variance],
_sub_mocap_odom.get().pose_covariance[y_variance]));
_mocap_epv = sqrtf(_sub_mocap_odom.get().pose_covariance[z_variance]);
if (PX4_ISFINITE(_sub_mocap_odom.get().position_variance[0])) {
// check if the mocap data is valid based on the variances
_mocap_eph = sqrtf(fmaxf(_sub_mocap_odom.get().position_variance[0], _sub_mocap_odom.get().position_variance[1]));
_mocap_epv = sqrtf(_sub_mocap_odom.get().position_variance[2]);
_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
_mocap_z_valid = _mocap_epv <= EP_MAX_STD_DEV;
@@ -87,11 +82,11 @@ int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
} else {
_time_last_mocap = _sub_mocap_odom.get().timestamp_sample;
if (PX4_ISFINITE(_sub_mocap_odom.get().x)) {
if (PX4_ISFINITE(_sub_mocap_odom.get().position[0])) {
y.setZero();
y(Y_mocap_x) = _sub_mocap_odom.get().x;
y(Y_mocap_y) = _sub_mocap_odom.get().y;
y(Y_mocap_z) = _sub_mocap_odom.get().z;
y(Y_mocap_x) = _sub_mocap_odom.get().position[0];
y(Y_mocap_y) = _sub_mocap_odom.get().position[1];
y(Y_mocap_z) = _sub_mocap_odom.get().position[2];
_mocapStats.update(y);
return OK;
src/modules/local_position_estimator/sensors/vision.cpp
+7 -12
View File
@@ -67,15 +67,10 @@ void BlockLocalPositionEstimator::visionInit()
int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
{
uint8_t x_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_X_VARIANCE;
uint8_t y_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_Y_VARIANCE;
uint8_t z_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_Z_VARIANCE;
if (PX4_ISFINITE(_sub_visual_odom.get().pose_covariance[x_variance])) {
if (PX4_ISFINITE(_sub_visual_odom.get().position_variance[0])) {
// check if the vision data is valid based on the covariances
_vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[x_variance],
_sub_visual_odom.get().pose_covariance[y_variance]));
_vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[z_variance]);
_vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().position_variance[0], _sub_visual_odom.get().position_variance[1]));
_vision_epv = sqrtf(_sub_visual_odom.get().position_variance[2]);
_vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV;
_vision_z_valid = _vision_epv <= EP_MAX_STD_DEV;
@@ -92,11 +87,11 @@ int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
} else {
_time_last_vision_p = _sub_visual_odom.get().timestamp_sample;
if (PX4_ISFINITE(_sub_visual_odom.get().x)) {
if (PX4_ISFINITE(_sub_visual_odom.get().position[0])) {
y.setZero();
y(Y_vision_x) = _sub_visual_odom.get().x;
y(Y_vision_y) = _sub_visual_odom.get().y;
y(Y_vision_z) = _sub_visual_odom.get().z;
y(Y_vision_x) = _sub_visual_odom.get().position[0];
y(Y_vision_y) = _sub_visual_odom.get().position[1];
y(Y_vision_z) = _sub_visual_odom.get().position[2];
_visionStats.update(y);
return OK;