dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 2022-08-04 12:55:21 -04:00 committed by GitHub
parent 61f390b0dd
commit dfdfbbfa9c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
21 changed files with 838 additions and 646 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
msg/vehicle_odometry.msg
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

16
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
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);

2
src/modules/ekf2/EKF/common.h
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{};

2
src/modules/ekf2/EKF/ekf.h
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); }

59
src/modules/ekf2/EKF/ekf_helper.cpp
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();
}

2
src/modules/ekf2/EKF/estimator_interface.h
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; }

231
src/modules/ekf2/EKF2.cpp
View File

@ -594,7 +594,7 @@ void EKF2::Run()
perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
PublishLocalPosition(now);
PublishOdometry(now, imu_sample_new);
PublishOdometry(now);
PublishGlobalPosition(now);
PublishWindEstimate(now);
@ -1059,72 +1059,43 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
_local_position_pub.publish(lpos);
}
void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
void EKF2::PublishOdometry(const hrt_abstime &timestamp)
{
// generate vehicle odometry data
vehicle_odometry_s odom;
odom.timestamp_sample = timestamp;
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// position
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
_ekf.getPosition().copyTo(odom.position);
// Vehicle odometry position
const Vector3f position{_ekf.getPosition()};
odom.x = position(0);
odom.y = position(1);
odom.z = position(2);
// Vehicle odometry linear velocity
odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
const Vector3f velocity{_ekf.getVelocity()};
odom.vx = velocity(0);
odom.vy = velocity(1);
odom.vz = velocity(2);
// Vehicle odometry quaternion
// orientation quaternion
_ekf.getQuaternion().copyTo(odom.q);
// Vehicle odometry angular rates
const Vector3f gyro_bias{_ekf.getGyroBias()};
const Vector3f rates{imu.delta_ang / imu.delta_ang_dt};
odom.rollspeed = rates(0) - gyro_bias(0);
odom.pitchspeed = rates(1) - gyro_bias(1);
odom.yawspeed = rates(2) - gyro_bias(2);
// velocity
odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
_ekf.getVelocity().copyTo(odom.velocity);
// get the covariance matrix size
static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
// angular_velocity
const Vector3f rates{_ekf.get_imu_sample_newest().delta_ang / _ekf.get_imu_sample_newest().delta_ang_dt};
const Vector3f angular_velocity = rates - _ekf.getGyroBias();
angular_velocity.copyTo(odom.angular_velocity);
// Get covariances to vehicle odometry
float covariances[24];
_ekf.covariances_diagonal().copyTo(covariances);
// velocity covariances
_ekf.velocity_covariances().diag().copyTo(odom.velocity_variance);
// initially set pose covariances to 0
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = 0.0;
}
// position covariances
_ekf.position_covariances().diag().copyTo(odom.position_variance);
// set the position variances
odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
// TODO: implement propagation from quaternion covariance to Euler angle covariance
// by employing the covariance law
// initially set velocity covariances to 0
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odom.velocity_covariance[i] = 0.0;
}
// set the linear velocity variances
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
// orientation covariance
_ekf.orientation_covariances_euler().diag().copyTo(odom.orientation_variance);
odom.reset_counter = _ekf.get_quat_reset_count()
+ _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
+ _ekf.get_posNE_reset_count() + _ekf.get_posD_reset_count();
odom.quality = 0;
// publish vehicle odometry data
odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_odometry_pub.publish(odom);
@ -1138,37 +1109,30 @@ void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_od
vehicle_odometry_s aligned_ev_odom{ev_odom};
// Rotate external position and velocity into EKF navigation frame
const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.x, ev_odom.y, ev_odom.z);
aligned_ev_odom.x = aligned_pos(0);
aligned_ev_odom.y = aligned_pos(1);
aligned_ev_odom.z = aligned_pos(2);
const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.position);
aligned_pos.copyTo(aligned_ev_odom.position);
switch (ev_odom.velocity_frame) {
case vehicle_odometry_s::BODY_FRAME_FRD: {
const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: {
const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.velocity);
aligned_vel.copyTo(aligned_ev_odom.velocity);
break;
}
case vehicle_odometry_s::LOCAL_FRAME_FRD: {
const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
case vehicle_odometry_s::VELOCITY_FRAME_FRD: {
const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.velocity);
aligned_vel.copyTo(aligned_ev_odom.velocity);
break;
}
}
aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
aligned_ev_odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
// Compute orientation in EKF navigation frame
Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q) ;
Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q);
ev_quat_aligned.normalize();
ev_quat_aligned.copyTo(aligned_ev_odom.q);
quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);
aligned_ev_odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
@ -1636,77 +1600,122 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
}
extVisionSample ev_data{};
ev_data.pos.setNaN();
ev_data.vel.setNaN();
ev_data.quat.setNaN();
// if error estimates are unavailable, use parameter defined defaults
// check for valid velocity data
if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) {
ev_data.vel(0) = ev_odom.vx;
ev_data.vel(1) = ev_odom.vy;
ev_data.vel(2) = ev_odom.vz;
if (PX4_ISFINITE(ev_odom.velocity[0]) && PX4_ISFINITE(ev_odom.velocity[1]) && PX4_ISFINITE(ev_odom.velocity[2])) {
bool velocity_valid = true;
if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
switch (ev_odom.velocity_frame) {
// case vehicle_odometry_s::VELOCITY_FRAME_NED:
// ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
// break;
} else {
case vehicle_odometry_s::VELOCITY_FRAME_FRD:
ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
break;
case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD:
ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
break;
default:
velocity_valid = false;
break;
}
// velocity measurement error from ev_data or parameters
float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
if (velocity_valid) {
ev_data.vel(0) = ev_odom.velocity[0];
ev_data.vel(1) = ev_odom.velocity[1];
ev_data.vel(2) = ev_odom.velocity[2];
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
&& PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
&& PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1];
ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2];
ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7];
ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
const float evv_noise_var = sq(_param_ekf2_evv_noise.get());
} else {
ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
// velocity measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() &&
PX4_ISFINITE(ev_odom.velocity_variance[0]) &&
PX4_ISFINITE(ev_odom.velocity_variance[1]) &&
PX4_ISFINITE(ev_odom.velocity_variance[2])) {
ev_data.velVar(0) = fmaxf(evv_noise_var, ev_odom.velocity_variance[0]);
ev_data.velVar(1) = fmaxf(evv_noise_var, ev_odom.velocity_variance[1]);
ev_data.velVar(2) = fmaxf(evv_noise_var, ev_odom.velocity_variance[2]);
} else {
ev_data.velVar.setAll(evv_noise_var);
}
new_ev_odom = true;
}
new_ev_odom = true;
}
// check for valid position data
if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
ev_data.pos(0) = ev_odom.x;
ev_data.pos(1) = ev_odom.y;
ev_data.pos(2) = ev_odom.z;
if (PX4_ISFINITE(ev_odom.position[0]) && PX4_ISFINITE(ev_odom.position[1]) && PX4_ISFINITE(ev_odom.position[2])) {
float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());
bool position_valid = true;
// position measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
&& PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
&& PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) {
ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);
// switch (ev_odom.pose_frame) {
// case vehicle_odometry_s::POSE_FRAME_NED:
// ev_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
// break;
} else {
ev_data.posVar.setAll(param_evp_noise_var);
// case vehicle_odometry_s::POSE_FRAME_FRD:
// ev_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
// break;
// default:
// position_valid = false;
// break;
// }
if (position_valid) {
ev_data.pos(0) = ev_odom.position[0];
ev_data.pos(1) = ev_odom.position[1];
ev_data.pos(2) = ev_odom.position[2];
const float evp_noise_var = sq(_param_ekf2_evp_noise.get());
// position measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() &&
PX4_ISFINITE(ev_odom.position_variance[0]) &&
PX4_ISFINITE(ev_odom.position_variance[1]) &&
PX4_ISFINITE(ev_odom.position_variance[2])
) {
ev_data.posVar(0) = fmaxf(evp_noise_var, ev_odom.position_variance[0]);
ev_data.posVar(1) = fmaxf(evp_noise_var, ev_odom.position_variance[1]);
ev_data.posVar(2) = fmaxf(evp_noise_var, ev_odom.position_variance[2]);
} else {
ev_data.posVar.setAll(evp_noise_var);
}
new_ev_odom = true;
}
new_ev_odom = true;
}
// check for valid orientation data
if (PX4_ISFINITE(ev_odom.q[0])) {
if ((PX4_ISFINITE(ev_odom.q[0]) && PX4_ISFINITE(ev_odom.q[1])
&& PX4_ISFINITE(ev_odom.q[2]) && PX4_ISFINITE(ev_odom.q[3]))
&& ((fabsf(ev_odom.q[0]) > 0.f) || (fabsf(ev_odom.q[1]) > 0.f)
|| (fabsf(ev_odom.q[2]) > 0.f) || (fabsf(ev_odom.q[3]) > 0.f))
) {
ev_data.quat = Quatf(ev_odom.q);
ev_data.quat.normalize();
// orientation measurement error from ev_data or parameters
float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());
const float eva_noise_var = sq(_param_ekf2_eva_noise.get());
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);
if (!_param_ekf2_ev_noise_md.get() &&
PX4_ISFINITE(ev_odom.orientation_variance[2])
) {
ev_data.angVar = fmaxf(eva_noise_var, ev_odom.orientation_variance[2]);
} else {
ev_data.angVar = param_eva_noise_var;
ev_data.angVar = eva_noise_var;
}
new_ev_odom = true;
@ -1714,6 +1723,8 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
// use timestamp from external computer, clocks are synchronized when using MAVROS
ev_data.time_us = ev_odom.timestamp_sample;
ev_data.reset_counter = ev_odom.reset_counter;
//ev_data.quality = ev_odom.quality;
if (new_ev_odom) {
_ekf.setExtVisionData(ev_data);

2
src/modules/ekf2/EKF2.hpp
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);

9
src/modules/ekf2/test/sensor_simulator/vio.cpp
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;

1
src/modules/ekf2/test/sensor_simulator/vio.h
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);

18
src/modules/ekf2/test/test_EKF_externalVision.cpp
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();

56
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
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();

21
src/modules/local_position_estimator/sensors/mocap.cpp
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;

19
src/modules/local_position_estimator/sensors/vision.cpp
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;

2
src/modules/mavlink/mavlink

@ -1 +1 @@
Subproject commit 05864e218e204f1ebeee5555988150fcddbd873e
Subproject commit c46af523263ff0dad59afe018fe387c1df030442

4
src/modules/mavlink/mavlink_messages.cpp
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

385
src/modules/mavlink/mavlink_receiver.cpp
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;
}
}

86
src/modules/mavlink/streams/ATT_POS_MOCAP.hpp
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

116
src/modules/mavlink/streams/ODOMETRY.hpp
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;

1
src/modules/simulator/simulator.h
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;

381
src/modules/simulator/simulator_mavlink.cpp
View File

@ -81,6 +81,22 @@ const unsigned mode_flag_custom = 1;
using namespace time_literals;
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
};
Simulator::Simulator()
: ModuleParams(nullptr)
{
@ -669,7 +685,219 @@ void Simulator::handle_message_landing_target(const mavlink_message_t *msg)
void Simulator::handle_message_odometry(const mavlink_message_t *msg)
{
publish_odometry_topic(msg);
mavlink_odometry_t odom_in;
mavlink_msg_odometry_decode(msg, &odom_in);
// fill vehicle_odometry from Mavlink ODOMETRY
vehicle_odometry_s odom{vehicle_odometry_empty};
odom.timestamp_sample = hrt_absolute_time(); // _mavlink_timesync.sync_stamp(odom_in.time_usec);
// 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;
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;
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;
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;
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;
}
}
}
// 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])) {
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];
// 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
}
}
// 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;
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: North
odom.velocity[1] = odom_in.vx; // x: East
odom.velocity[2] = -odom_in.vz; // z: Up
break;
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;
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;
}
// 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;
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;
default:
// unsupported child_frame_id
break;
}
}
}
// 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:
PX4_ERR("ODOMETRY: estimator_type %" PRIu8 " unsupported", odom_in.estimator_type);
return;
}
}
void Simulator::handle_message_optical_flow(const mavlink_message_t *msg)
@ -753,7 +981,39 @@ void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg)
{
publish_odometry_topic(msg);
mavlink_vision_position_estimate_t vpe;
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
vehicle_odometry_s odom{vehicle_odometry_empty};
odom.timestamp_sample = hrt_absolute_time(); // _mavlink_timesync.sync_stamp(vpe.usec);
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = vpe.x;
odom.position[1] = vpe.y;
odom.position[2] = vpe.z;
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
q.copyTo(odom.q);
// 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
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
odom.reset_counter = vpe.reset_counter;
odom.timestamp = hrt_absolute_time();
_visual_odometry_pub.publish(odom);
}
void Simulator::send_mavlink_message(const mavlink_message_t &aMsg)
@ -1358,123 +1618,6 @@ void Simulator::check_failure_injections()
}
}
int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
struct vehicle_odometry_s odom;
odom.timestamp = timestamp;
odom.timestamp_sample = timestamp;
const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
if (odom_mavlink->msgid == MAVLINK_MSG_ID_ODOMETRY) {
mavlink_odometry_t odom_msg;
mavlink_msg_odometry_decode(odom_mavlink, &odom_msg);
/* The position in the local NED frame */
odom.x = odom_msg.x;
odom.y = odom_msg.y;
odom.z = odom_msg.z;
/* The quaternion of the ODOMETRY msg represents a rotation from
* NED earth/local frame to XYZ body frame */
matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
q.copyTo(odom.q);
if (odom_msg.frame_id == MAV_FRAME_LOCAL_NED) {
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
} else {
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
}
static_assert(POS_URT_SIZE == (sizeof(odom_msg.pose_covariance) / sizeof(odom_msg.pose_covariance[0])),
"Odometry Pose Covariance matrix URT array size mismatch");
/* The pose covariance URT */
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = odom_msg.pose_covariance[i];
}
/* The velocity in the body-fixed frame */
odom.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD;
odom.vx = odom_msg.vx;
odom.vy = odom_msg.vy;
odom.vz = odom_msg.vz;
/* The angular velocity in the body-fixed frame */
odom.rollspeed = odom_msg.rollspeed;
odom.pitchspeed = odom_msg.pitchspeed;
odom.yawspeed = odom_msg.yawspeed;
// velocity_covariance
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
static_assert(VEL_URT_SIZE == (sizeof(odom_msg.velocity_covariance) / sizeof(odom_msg.velocity_covariance[0])),
"Odometry Velocity Covariance matrix URT array size mismatch");
/* The velocity covariance URT */
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odom.velocity_covariance[i] = odom_msg.velocity_covariance[i];
}
odom.reset_counter = odom_msg.reset_counter;
/* Publish the odometry based on the source */
if (odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VISION || odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_VIO) {
_visual_odometry_pub.publish(odom);
} else if (odom_msg.estimator_type == MAV_ESTIMATOR_TYPE_MOCAP) {
_mocap_odometry_pub.publish(odom);
} else {
PX4_ERR("Estimator source %u not supported. Unable to publish pose and velocity", odom_msg.estimator_type);
}
} else if (odom_mavlink->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
mavlink_vision_position_estimate_t ev;
mavlink_msg_vision_position_estimate_decode(odom_mavlink, &ev);
/* The position in the local NED frame */
odom.x = ev.x;
odom.y = ev.y;
odom.z = ev.z;
/* The euler angles of the VISUAL_POSITION_ESTIMATE msg represent a
* rotation from NED earth/local frame to XYZ body frame */
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(odom.q);
odom.local_frame = vehicle_odometry_s::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");
/* The pose covariance URT */
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = ev.covariance[i];
}
/* The velocity in the local NED frame - unknown */
odom.vx = NAN;
odom.vy = NAN;
odom.vz = NAN;
/* The angular velocity in body-fixed frame - unknown */
odom.rollspeed = NAN;
odom.pitchspeed = NAN;
odom.yawspeed = NAN;
/* The velocity covariance URT - unknown */
odom.velocity_covariance[0] = NAN;
odom.reset_counter = ev.reset_counter;
/* Publish the odometry */
_visual_odometry_pub.publish(odom);
}
return PX4_OK;
}
int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink)
{
distance_sensor_s dist{};