Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-29 12:32:05 +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
msg/vehicle_odometry.msg
+17 -54
View File
@@ -1,68 +1,31 @@
# Vehicle odometry data. Fits ROS REP 147 for aerial vehicles
uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample
# Covariance matrix index constants
uint8 COVARIANCE_MATRIX_X_VARIANCE=0
uint8 COVARIANCE_MATRIX_Y_VARIANCE=6
uint8 COVARIANCE_MATRIX_Z_VARIANCE=11
uint8 COVARIANCE_MATRIX_ROLL_VARIANCE=15
uint8 COVARIANCE_MATRIX_PITCH_VARIANCE=18
uint8 COVARIANCE_MATRIX_YAW_VARIANCE=20
uint8 COVARIANCE_MATRIX_VX_VARIANCE=0
uint8 COVARIANCE_MATRIX_VY_VARIANCE=6
uint8 COVARIANCE_MATRIX_VZ_VARIANCE=11
uint8 COVARIANCE_MATRIX_ROLLRATE_VARIANCE=15
uint8 COVARIANCE_MATRIX_PITCHRATE_VARIANCE=18
uint8 COVARIANCE_MATRIX_YAWRATE_VARIANCE=20
uint8 POSE_FRAME_UNKNOWN = 0
uint8 POSE_FRAME_NED = 1 # NED earth-fixed frame
uint8 POSE_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference
uint8 pose_frame # Position and orientation frame of reference
# Position and linear velocity frame of reference constants
uint8 LOCAL_FRAME_NED=0 # NED earth-fixed frame
uint8 LOCAL_FRAME_FRD=1 # FRD earth-fixed frame, arbitrary heading reference
uint8 LOCAL_FRAME_OTHER=2 # Not aligned with the std frames of reference
uint8 BODY_FRAME_FRD=3 # FRD body-fixed frame
float32[3] position # Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown
float32[4] q # Quaternion rotation from FRD body frame to reference frame. First value NaN if invalid/unknown
# Position and linear velocity local frame of reference
uint8 local_frame
uint8 VELOCITY_FRAME_UNKNOWN = 0
uint8 VELOCITY_FRAME_NED = 1 # NED earth-fixed frame
uint8 VELOCITY_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference
uint8 VELOCITY_FRAME_BODY_FRD = 3 # FRD body-fixed frame
uint8 velocity_frame # Reference frame of the velocity data
# 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
float32[3] velocity # Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown
# Orientation quaternion. First value NaN if invalid/unknown
float32[4] q # Quaternion rotation from FRD body frame to reference frame
float32[4] q_offset # Quaternion rotation from odometry reference frame to navigation frame
float32[3] angular_velocity # Angular velocity in body-fixed frame (rad/s). NaN if invalid/unknown
# Row-major representation of 6x6 pose cross-covariance matrix URT.
# NED earth-fixed frame.
# Order: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis
# If position covariance invalid/unknown, first cell is NaN
# If orientation covariance invalid/unknown, 16th cell is NaN
float32[21] pose_covariance
# Reference frame of the velocity data
uint8 velocity_frame
# Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown
float32 vx # North velocity
float32 vy # East velocity
float32 vz # Down velocity
# Angular rate in body-fixed frame (rad/s). NaN if invalid/unknown
float32 rollspeed # Angular velocity about X body axis
float32 pitchspeed # Angular velocity about Y body axis
float32 yawspeed # Angular velocity about Z body axis
# Row-major representation of 6x6 velocity cross-covariance matrix URT.
# Linear velocity in NED earth-fixed frame. Angular velocity in body-fixed frame.
# Order: vx, vy, vz, rotation rate about X axis, rotation rate about Y axis, rotation rate about Z axis
# If linear velocity covariance invalid/unknown, first cell is NaN
# If angular velocity covariance invalid/unknown, 16th cell is NaN
float32[21] velocity_covariance
float32[3] position_variance
float32[3] orientation_variance
float32[3] velocity_variance
uint8 reset_counter
int8 quality
# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
# TOPICS estimator_odometry estimator_visual_odometry_aligned
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+8 -8
View File
@@ -252,10 +252,10 @@ void AttitudeEstimatorQ::update_motion_capture_odometry()
if (_vehicle_mocap_odometry_sub.update(&mocap)) {
// validation check for mocap attitude data
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
&& (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
&& (PX4_ISFINITE(mocap.orientation_variance[0]) ? sqrtf(fmaxf(
mocap.orientation_variance[0],
fmaxf(mocap.orientation_variance[1],
mocap.orientation_variance[2]))) <= _eo_max_std_dev : true);
if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q);
@@ -361,10 +361,10 @@ void AttitudeEstimatorQ::update_visual_odometry()
if (_vehicle_visual_odometry_sub.update(&vision)) {
// validation check for vision attitude data
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
&& (PX4_ISFINITE(vision.orientation_variance[0]) ? sqrtf(fmaxf(
vision.orientation_variance[0],
fmaxf(vision.orientation_variance[1],
vision.orientation_variance[2]))) <= _eo_max_std_dev : true);
if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q);
src/modules/ekf2/EKF/common.h
+1 -1
View File
@@ -207,7 +207,7 @@ struct extVisionSample {
Vector3f vel{}; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis
Quatf quat{}; ///< quaternion defining rotation from body to earth frame
Vector3f posVar{}; ///< XYZ position variances (m**2)
Matrix3f velCov{}; ///< XYZ velocity covariances ((m/sec)**2)
Vector3f velVar{}; ///< XYZ velocity variances ((m/sec)**2)
float angVar{}; ///< angular heading variance (rad**2)
VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
uint8_t reset_counter{};
src/modules/ekf2/EKF/ekf.h
+2
View File
@@ -201,6 +201,8 @@ public:
// get the orientation (quaterion) covariances
matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<4, 4>(0, 0); }
matrix::SquareMatrix<float, 3> orientation_covariances_euler() const;
// get the linear velocity covariances
matrix::SquareMatrix<float, 3> velocity_covariances() const { return P.slice<3, 3>(4, 4); }
src/modules/ekf2/EKF/ekf_helper.cpp
+58 -1
View File
@@ -1508,7 +1508,7 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const
Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const
{
Matrix3f ev_vel_cov = _ev_sample_delayed.velCov;
Matrix3f ev_vel_cov = matrix::diag(_ev_sample_delayed.velVar);
// rotate measurement into correct earth frame if required
switch (_ev_sample_delayed.vel_frame) {
@@ -1903,3 +1903,60 @@ void Ekf::resetGpsDriftCheckFilters()
_gps_vertical_position_drift_rate_m_s = NAN;
_gps_filtered_horizontal_velocity_m_s = NAN;
}
matrix::SquareMatrix<float, 3> Ekf::orientation_covariances_euler() const
{
// Jacobian matrix (3x4) containing the partial derivatives of the
// Euler angle equations with respect to the quaternions
matrix::Matrix<float, 3, 4> G;
// quaternion components
float q1 = _state.quat_nominal(0);
float q2 = _state.quat_nominal(1);
float q3 = _state.quat_nominal(2);
float q4 = _state.quat_nominal(3);
// numerator components
float n1 = 2 * q1 * q2 + 2 * q2 * q4;
float n2 = -2 * q2 * q2 - 2 * q3 * q3 + 1;
float n3 = 2 * q1 * q4 + 2 * q2 * q3;
float n4 = -2 * q3 * q3 - 2 * q4 * q4 + 1;
float n5 = 2 * q1 * q3 + 2 * q2 * q4;
float n6 = -2 * q1 * q2 - 2 * q2 * q4;
float n7 = -2 * q1 * q4 - 2 * q2 * q3;
// Protect against division by 0
float d1 = n1 * n1 + n2 * n2;
float d2 = n3 * n3 + n4 * n4;
if (fabsf(d1) < FLT_EPSILON) {
d1 = FLT_EPSILON;
}
if (fabsf(d2) < FLT_EPSILON) {
d2 = FLT_EPSILON;
}
// Protect against square root of negative numbers
float x = math::max(-n5 * n5 + 1, 0.0f);
// compute G matrix
float sqrt_x = sqrtf(x);
float g00_03 = 2 * q2 * n2 / d1;
G(0, 0) = g00_03;
G(0, 1) = -4 * q2 * n6 / d1 + (2 * q1 + 2 * q4) * n2 / d1;
G(0, 2) = -4 * q3 * n6 / d1;
G(0, 3) = g00_03;
G(1, 0) = 2 * q3 / sqrt_x;
G(1, 1) = 2 * q4 / sqrt_x;
G(1, 2) = 2 * q1 / sqrt_x;
G(1, 3) = 2 * q2 / sqrt_x;
G(2, 0) = 2 * q4 * n4 / d2;
G(2, 1) = 2 * q3 * n4 / d2;
G(2, 2) = 2 * q2 * n4 / d2 - 4 * q3 * n7 / d2;
G(2, 3) = 2 * q1 * n4 / d2 - 4 * q4 * n7 / d2;
const matrix::SquareMatrix<float, 4> quat_covariances = P.slice<4, 4>(0, 0);
return G * quat_covariances * G.transpose();
}
src/modules/ekf2/EKF/estimator_interface.h
+2
View File
@@ -245,6 +245,8 @@ public:
// Getters for samples on the delayed time horizon
const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
const imuSample &get_imu_sample_newest() const { return _newest_high_rate_imu_sample; }
const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
src/modules/ekf2/EKF2.cpp
+121 -110
View File
File diff suppressed because it is too large Load Diff
src/modules/ekf2/EKF2.hpp
+1 -1
View File
@@ -144,7 +144,7 @@ private:
void PublishInnovationTestRatios(const hrt_abstime &timestamp);
void PublishInnovationVariances(const hrt_abstime &timestamp);
void PublishLocalPosition(const hrt_abstime &timestamp);
void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu);
void PublishOdometry(const hrt_abstime &timestamp);
void PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom);
void PublishOpticalFlowVel(const hrt_abstime &timestamp);
void PublishSensorBias(const hrt_abstime &timestamp);
src/modules/ekf2/test/sensor_simulator/vio.cpp
+2 -7
View File
@@ -26,12 +26,7 @@ void Vio::setData(const extVisionSample &vio_data)
void Vio::setVelocityVariance(const Vector3f &velVar)
{
setVelocityCovariance(matrix::diag(velVar));
}
void Vio::setVelocityCovariance(const Matrix3f &velCov)
{
_vio_data.velCov = velCov;
_vio_data.velVar = velVar;
}
void Vio::setPositionVariance(const Vector3f &posVar)
@@ -76,7 +71,7 @@ extVisionSample Vio::dataAtRest()
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velCov = matrix::eye<float, 3>() * 0.1f;
vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.angVar = 0.05f;
vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
return vio_data;
src/modules/ekf2/test/sensor_simulator/vio.h
-1
View File
@@ -53,7 +53,6 @@ public:
void setData(const extVisionSample &vio_data);
void setVelocityVariance(const Vector3f &velVar);
void setVelocityCovariance(const Matrix3f &velCov);
void setPositionVariance(const Vector3f &posVar);
void setAngularVariance(float angVar);
void setVelocity(const Vector3f &vel);
src/modules/ekf2/test/test_EKF_externalVision.cpp
+6 -12
View File
@@ -276,13 +276,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameBody)
// WHEN: measurement is given in BODY-FRAME and
// x variance is bigger than y variance
_sensor_simulator._vio.setVelocityFrameToBody();
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
0.0f, 0.01f, 0.0f,
0.0f, 0.0f, 0.01f
};
const Matrix3f vel_cov_body(vel_cov_data);
const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
const Vector3f vel_body(1.0f, 0.0f, 0.0f);
_sensor_simulator._vio.setVelocityCovariance(vel_cov_body);
_sensor_simulator._vio.setVelocityVariance(vel_cov_body);
_sensor_simulator._vio.setVelocity(vel_body);
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision();
@@ -312,13 +309,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
// WHEN: measurement is given in LOCAL-FRAME and
// x variance is bigger than y variance
_sensor_simulator._vio.setVelocityFrameToLocal();
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
0.0f, 0.01f, 0.0f,
0.0f, 0.0f, 0.01f
};
const Matrix3f vel_cov_earth(vel_cov_data);
const Vector3f vel_cov_earth{2.f, 0.01f, 0.01f};
const Vector3f vel_earth(1.0f, 0.0f, 0.0f);
_sensor_simulator._vio.setVelocityCovariance(vel_cov_earth);
_sensor_simulator._vio.setVelocityVariance(vel_cov_earth);
_sensor_simulator._vio.setVelocity(vel_earth);
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision();
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+25 -31
View File
@@ -649,17 +649,17 @@ void BlockLocalPositionEstimator::publishOdom()
&& PX4_ISFINITE(_x(X_vz))) {
_pub_odom.get().timestamp_sample = _timeStamp;
_pub_odom.get().local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
_pub_odom.get().pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
// position
_pub_odom.get().x = xLP(X_x); // north
_pub_odom.get().y = xLP(X_y); // east
_pub_odom.get().position[0] = xLP(X_x); // north
_pub_odom.get().position[1] = xLP(X_y); // east
if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) {
_pub_odom.get().z = -_aglLowPass.getState(); // agl
_pub_odom.get().position[2] = -_aglLowPass.getState(); // agl
} else {
_pub_odom.get().z = xLP(X_z); // down
_pub_odom.get().position[2] = xLP(X_z); // down
}
// orientation
@@ -667,51 +667,45 @@ void BlockLocalPositionEstimator::publishOdom()
q.copyTo(_pub_odom.get().q);
// linear velocity
_pub_odom.get().velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
_pub_odom.get().vx = xLP(X_vx); // vel north
_pub_odom.get().vy = xLP(X_vy); // vel east
_pub_odom.get().vz = xLP(X_vz); // vel down
_pub_odom.get().velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD;
_pub_odom.get().velocity[0] = xLP(X_vx); // vel north
_pub_odom.get().velocity[1] = xLP(X_vy); // vel east
_pub_odom.get().velocity[2] = xLP(X_vz); // vel down
// angular velocity
_pub_odom.get().rollspeed = _sub_angular_velocity.get().xyz[0]; // roll rate
_pub_odom.get().pitchspeed = _sub_angular_velocity.get().xyz[1]; // pitch rate
_pub_odom.get().yawspeed = _sub_angular_velocity.get().xyz[2]; // yaw rate
_pub_odom.get().angular_velocity[0] = NAN;
_pub_odom.get().angular_velocity[1] = NAN;
_pub_odom.get().angular_velocity[2] = NAN;
// get the covariance matrix size
const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]);
const size_t VEL_URT_SIZE = sizeof(_pub_odom.get().velocity_covariance) / sizeof(
_pub_odom.get().velocity_covariance[0]);
const size_t POS_URT_SIZE = sizeof(_pub_odom.get().position_variance) / sizeof(_pub_odom.get().position_variance[0]);
const size_t VEL_URT_SIZE = sizeof(_pub_odom.get().velocity_variance) / sizeof(_pub_odom.get().velocity_variance[0]);
// initially set pose covariances to 0
for (size_t i = 0; i < POS_URT_SIZE; i++) {
_pub_odom.get().pose_covariance[i] = 0.0;
_pub_odom.get().position_variance[i] = NAN;
}
// set the position variances
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_X_VARIANCE] = m_P(X_vx, X_vx);
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Y_VARIANCE] = m_P(X_vy, X_vy);
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_Z_VARIANCE] = m_P(X_vz, X_vz);
_pub_odom.get().position_variance[0] = m_P(X_vx, X_vx);
_pub_odom.get().position_variance[1] = m_P(X_vy, X_vy);
_pub_odom.get().position_variance[2] = m_P(X_vz, X_vz);
// unknown orientation covariances
// TODO: add orientation covariance to vehicle_attitude
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLL_VARIANCE] = NAN;
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCH_VARIANCE] = NAN;
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAW_VARIANCE] = NAN;
_pub_odom.get().orientation_variance[0] = NAN;
_pub_odom.get().orientation_variance[1] = NAN;
_pub_odom.get().orientation_variance[2] = NAN;
// initially set velocity covariances to 0
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
_pub_odom.get().velocity_covariance[i] = 0.0;
_pub_odom.get().velocity_variance[i] = NAN;
}
// set the linear velocity variances
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VX_VARIANCE] = m_P(X_vx, X_vx);
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VY_VARIANCE] = m_P(X_vy, X_vy);
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = m_P(X_vz, X_vz);
// unknown angular velocity covariances
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLLRATE_VARIANCE] = NAN;
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCHRATE_VARIANCE] = NAN;
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAWRATE_VARIANCE] = NAN;
_pub_odom.get().velocity_variance[0] = m_P(X_vx, X_vx);
_pub_odom.get().velocity_variance[1] = m_P(X_vy, X_vy);
_pub_odom.get().velocity_variance[2] = m_P(X_vz, X_vz);
_pub_odom.get().timestamp = hrt_absolute_time();
_pub_odom.update();
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;
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
File diff suppressed because it is too large Load Diff
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;
src/modules/simulator/simulator.h
-1
View File
@@ -159,7 +159,6 @@ private:
void check_failure_injections();
int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
int publish_distance_topic(const mavlink_distance_sensor_t *dist);
static Simulator *_instance;
src/modules/simulator/simulator_mavlink.cpp
+262 -119
View File
File diff suppressed because it is too large Load Diff