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 # Vehicle odometry data. Fits ROS REP 147 for aerial vehicles
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample uint64 timestamp_sample
# Covariance matrix index constants uint8 POSE_FRAME_UNKNOWN = 0
uint8 COVARIANCE_MATRIX_X_VARIANCE=0 uint8 POSE_FRAME_NED = 1 # NED earth-fixed frame
uint8 COVARIANCE_MATRIX_Y_VARIANCE=6 uint8 POSE_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference
uint8 COVARIANCE_MATRIX_Z_VARIANCE=11 uint8 pose_frame # Position and orientation frame of reference
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
# Position and linear velocity frame of reference constants float32[3] position # Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown
uint8 LOCAL_FRAME_NED=0 # NED earth-fixed frame float32[4] q # Quaternion rotation from FRD body frame to reference frame. First value NaN if invalid/unknown
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
# Position and linear velocity local frame of reference uint8 VELOCITY_FRAME_UNKNOWN = 0
uint8 local_frame 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[3] velocity # Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown
float32 x # North position
float32 y # East position
float32 z # Down position
# Orientation quaternion. First value NaN if invalid/unknown float32[3] angular_velocity # Angular velocity in body-fixed frame (rad/s). 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
# Row-major representation of 6x6 pose cross-covariance matrix URT. float32[3] position_variance
# NED earth-fixed frame. float32[3] orientation_variance
# Order: x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis float32[3] velocity_variance
# 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
uint8 reset_counter uint8 reset_counter
int8 quality
# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
# TOPICS estimator_odometry estimator_visual_odometry_aligned # 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)) { if (_vehicle_mocap_odometry_sub.update(&mocap)) {
// validation check for mocap attitude data // validation check for mocap attitude data
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0]) bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
&& (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf( && (PX4_ISFINITE(mocap.orientation_variance[0]) ? sqrtf(fmaxf(
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE], mocap.orientation_variance[0],
fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE], fmaxf(mocap.orientation_variance[1],
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true); mocap.orientation_variance[2]))) <= _eo_max_std_dev : true);
if (mocap_att_valid) { if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q); Dcmf Rmoc = Quatf(mocap.q);
@ -361,10 +361,10 @@ void AttitudeEstimatorQ::update_visual_odometry()
if (_vehicle_visual_odometry_sub.update(&vision)) { if (_vehicle_visual_odometry_sub.update(&vision)) {
// validation check for vision attitude data // validation check for vision attitude data
bool vision_att_valid = PX4_ISFINITE(vision.q[0]) bool vision_att_valid = PX4_ISFINITE(vision.q[0])
&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf( && (PX4_ISFINITE(vision.orientation_variance[0]) ? sqrtf(fmaxf(
vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE], vision.orientation_variance[0],
fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE], fmaxf(vision.orientation_variance[1],
vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true); vision.orientation_variance[2]))) <= _eo_max_std_dev : true);
if (vision_att_valid) { if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q); 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 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 Quatf quat{}; ///< quaternion defining rotation from body to earth frame
Vector3f posVar{}; ///< XYZ position variances (m**2) 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) float angVar{}; ///< angular heading variance (rad**2)
VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD; VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD;
uint8_t reset_counter{}; uint8_t reset_counter{};

2
src/modules/ekf2/EKF/ekf.h
View File

@ -201,6 +201,8 @@ public:
// get the orientation (quaterion) covariances // get the orientation (quaterion) covariances
matrix::SquareMatrix<float, 4> orientation_covariances() const { return P.slice<4, 4>(0, 0); } 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 // get the linear velocity covariances
matrix::SquareMatrix<float, 3> velocity_covariances() const { return P.slice<3, 3>(4, 4); } 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 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 // rotate measurement into correct earth frame if required
switch (_ev_sample_delayed.vel_frame) { switch (_ev_sample_delayed.vel_frame) {
@ -1903,3 +1903,60 @@ void Ekf::resetGpsDriftCheckFilters()
_gps_vertical_position_drift_rate_m_s = NAN; _gps_vertical_position_drift_rate_m_s = NAN;
_gps_filtered_horizontal_velocity_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 // Getters for samples on the delayed time horizon
const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; } 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 baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }

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

2
src/modules/ekf2/EKF2.hpp
View File

@ -144,7 +144,7 @@ private:
void PublishInnovationTestRatios(const hrt_abstime &timestamp); void PublishInnovationTestRatios(const hrt_abstime &timestamp);
void PublishInnovationVariances(const hrt_abstime &timestamp); void PublishInnovationVariances(const hrt_abstime &timestamp);
void PublishLocalPosition(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 PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom);
void PublishOpticalFlowVel(const hrt_abstime &timestamp); void PublishOpticalFlowVel(const hrt_abstime &timestamp);
void PublishSensorBias(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) void Vio::setVelocityVariance(const Vector3f &velVar)
{ {
setVelocityCovariance(matrix::diag(velVar)); _vio_data.velVar = velVar;
}
void Vio::setVelocityCovariance(const Matrix3f &velCov)
{
_vio_data.velCov = velCov;
} }
void Vio::setPositionVariance(const Vector3f &posVar) 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.vel = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.quat = Quatf{1.0f, 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.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.angVar = 0.05f;
vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
return vio_data; 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 setData(const extVisionSample &vio_data);
void setVelocityVariance(const Vector3f &velVar); void setVelocityVariance(const Vector3f &velVar);
void setVelocityCovariance(const Matrix3f &velCov);
void setPositionVariance(const Vector3f &posVar); void setPositionVariance(const Vector3f &posVar);
void setAngularVariance(float angVar); void setAngularVariance(float angVar);
void setVelocity(const Vector3f &vel); 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 // WHEN: measurement is given in BODY-FRAME and
// x variance is bigger than y variance // x variance is bigger than y variance
_sensor_simulator._vio.setVelocityFrameToBody(); _sensor_simulator._vio.setVelocityFrameToBody();
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
0.0f, 0.01f, 0.0f, const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f);
0.0f, 0.0f, 0.01f
};
const Matrix3f vel_cov_body(vel_cov_data);
const Vector3f vel_body(1.0f, 0.0f, 0.0f); 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); _sensor_simulator._vio.setVelocity(vel_body);
_ekf_wrapper.enableExternalVisionVelocityFusion(); _ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision(); _sensor_simulator.startExternalVision();
@ -312,13 +309,10 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal)
// WHEN: measurement is given in LOCAL-FRAME and // WHEN: measurement is given in LOCAL-FRAME and
// x variance is bigger than y variance // x variance is bigger than y variance
_sensor_simulator._vio.setVelocityFrameToLocal(); _sensor_simulator._vio.setVelocityFrameToLocal();
float vel_cov_data [9] = {2.0f, 0.0f, 0.0f,
0.0f, 0.01f, 0.0f, const Vector3f vel_cov_earth{2.f, 0.01f, 0.01f};
0.0f, 0.0f, 0.01f
};
const Matrix3f vel_cov_earth(vel_cov_data);
const Vector3f vel_earth(1.0f, 0.0f, 0.0f); 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); _sensor_simulator._vio.setVelocity(vel_earth);
_ekf_wrapper.enableExternalVisionVelocityFusion(); _ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator.startExternalVision(); _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))) { && PX4_ISFINITE(_x(X_vz))) {
_pub_odom.get().timestamp_sample = _timeStamp; _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 // position
_pub_odom.get().x = xLP(X_x); // north _pub_odom.get().position[0] = xLP(X_x); // north
_pub_odom.get().y = xLP(X_y); // east _pub_odom.get().position[1] = xLP(X_y); // east
if (_param_lpe_fusion.get() & FUSE_PUB_AGL_Z) { 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 { } else {
_pub_odom.get().z = xLP(X_z); // down _pub_odom.get().position[2] = xLP(X_z); // down
} }
// orientation // orientation
@ -667,51 +667,45 @@ void BlockLocalPositionEstimator::publishOdom()
q.copyTo(_pub_odom.get().q); q.copyTo(_pub_odom.get().q);
// linear velocity // linear velocity
_pub_odom.get().velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; _pub_odom.get().velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_FRD;
_pub_odom.get().vx = xLP(X_vx); // vel north _pub_odom.get().velocity[0] = xLP(X_vx); // vel north
_pub_odom.get().vy = xLP(X_vy); // vel east _pub_odom.get().velocity[1] = xLP(X_vy); // vel east
_pub_odom.get().vz = xLP(X_vz); // vel down _pub_odom.get().velocity[2] = xLP(X_vz); // vel down
// angular velocity // angular velocity
_pub_odom.get().rollspeed = _sub_angular_velocity.get().xyz[0]; // roll rate _pub_odom.get().angular_velocity[0] = NAN;
_pub_odom.get().pitchspeed = _sub_angular_velocity.get().xyz[1]; // pitch rate _pub_odom.get().angular_velocity[1] = NAN;
_pub_odom.get().yawspeed = _sub_angular_velocity.get().xyz[2]; // yaw rate _pub_odom.get().angular_velocity[2] = NAN;
// get the covariance matrix size // 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 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_covariance) / sizeof( const size_t VEL_URT_SIZE = sizeof(_pub_odom.get().velocity_variance) / sizeof(_pub_odom.get().velocity_variance[0]);
_pub_odom.get().velocity_covariance[0]);
// initially set pose covariances to 0 // initially set pose covariances to 0
for (size_t i = 0; i < POS_URT_SIZE; i++) { 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 // 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().position_variance[0] = 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().position_variance[1] = 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[2] = m_P(X_vz, X_vz);
// unknown orientation covariances // unknown orientation covariances
// TODO: add orientation covariance to vehicle_attitude // TODO: add orientation covariance to vehicle_attitude
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_ROLL_VARIANCE] = NAN; _pub_odom.get().orientation_variance[0] = NAN;
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_PITCH_VARIANCE] = NAN; _pub_odom.get().orientation_variance[1] = NAN;
_pub_odom.get().pose_covariance[_pub_odom.get().COVARIANCE_MATRIX_YAW_VARIANCE] = NAN; _pub_odom.get().orientation_variance[2] = NAN;
// initially set velocity covariances to 0 // initially set velocity covariances to 0
for (size_t i = 0; i < VEL_URT_SIZE; i++) { 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 // 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_variance[0] = 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_variance[1] = m_P(X_vy, X_vy);
_pub_odom.get().velocity_covariance[_pub_odom.get().COVARIANCE_MATRIX_VZ_VARIANCE] = m_P(X_vz, X_vz); _pub_odom.get().velocity_variance[2] = 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().timestamp = hrt_absolute_time(); _pub_odom.get().timestamp = hrt_absolute_time();
_pub_odom.update(); _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) int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
{ {
uint8_t x_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_X_VARIANCE; if (PX4_ISFINITE(_sub_mocap_odom.get().position_variance[0])) {
uint8_t y_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Y_VARIANCE; // check if the mocap data is valid based on the variances
uint8_t z_variance = _sub_mocap_odom.get().COVARIANCE_MATRIX_Z_VARIANCE; _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]);
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]);
_mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV; _mocap_xy_valid = _mocap_eph <= EP_MAX_STD_DEV;
_mocap_z_valid = _mocap_epv <= 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 { } else {
_time_last_mocap = _sub_mocap_odom.get().timestamp_sample; _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.setZero();
y(Y_mocap_x) = _sub_mocap_odom.get().x; y(Y_mocap_x) = _sub_mocap_odom.get().position[0];
y(Y_mocap_y) = _sub_mocap_odom.get().y; y(Y_mocap_y) = _sub_mocap_odom.get().position[1];
y(Y_mocap_z) = _sub_mocap_odom.get().z; y(Y_mocap_z) = _sub_mocap_odom.get().position[2];
_mocapStats.update(y); _mocapStats.update(y);
return OK; 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) int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
{ {
uint8_t x_variance = _sub_visual_odom.get().COVARIANCE_MATRIX_X_VARIANCE; if (PX4_ISFINITE(_sub_visual_odom.get().position_variance[0])) {
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])) {
// check if the vision data is valid based on the covariances // check if the vision data is valid based on the covariances
_vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().pose_covariance[x_variance], _vision_eph = sqrtf(fmaxf(_sub_visual_odom.get().position_variance[0], _sub_visual_odom.get().position_variance[1]));
_sub_visual_odom.get().pose_covariance[y_variance])); _vision_epv = sqrtf(_sub_visual_odom.get().position_variance[2]);
_vision_epv = sqrtf(_sub_visual_odom.get().pose_covariance[z_variance]);
_vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV; _vision_xy_valid = _vision_eph <= EP_MAX_STD_DEV;
_vision_z_valid = _vision_epv <= 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 { } else {
_time_last_vision_p = _sub_visual_odom.get().timestamp_sample; _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.setZero();
y(Y_vision_x) = _sub_visual_odom.get().x; y(Y_vision_x) = _sub_visual_odom.get().position[0];
y(Y_vision_y) = _sub_visual_odom.get().y; y(Y_vision_y) = _sub_visual_odom.get().position[1];
y(Y_vision_z) = _sub_visual_odom.get().z; y(Y_vision_z) = _sub_visual_odom.get().position[2];
_visionStats.update(y); _visionStats.update(y);
return OK; 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) #if !defined(CONSTRAINED_FLASH)
# include "streams/ADSB_VEHICLE.hpp" # include "streams/ADSB_VEHICLE.hpp"
# include "streams/ATT_POS_MOCAP.hpp"
# include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp" # include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp"
# include "streams/DEBUG.hpp" # include "streams/DEBUG.hpp"
# include "streams/DEBUG_FLOAT_ARRAY.hpp" # include "streams/DEBUG_FLOAT_ARRAY.hpp"
@ -401,9 +400,6 @@ static const StreamListItem streams_list[] = {
#if defined(VIBRATION_HPP) #if defined(VIBRATION_HPP)
create_stream_list_item<MavlinkStreamVibration>(), create_stream_list_item<MavlinkStreamVibration>(),
#endif // VIBRATION_HPP #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) #if defined(AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP)
create_stream_list_item<MavlinkStreamAutopilotStateForGimbalDevice>(), create_stream_list_item<MavlinkStreamAutopilotStateForGimbalDevice>(),
#endif // AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP #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 #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) : MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
ModuleParams(nullptr), ModuleParams(nullptr),
_mavlink(parent), _mavlink(parent),
@ -948,40 +964,39 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
void void
MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
{ {
mavlink_att_pos_mocap_t mocap; mavlink_att_pos_mocap_t att_pos_mocap;
mavlink_msg_att_pos_mocap_decode(msg, &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(); odom.timestamp_sample = _mavlink_timesync.sync_stamp(att_pos_mocap.time_usec);
mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec);
mocap_odom.x = mocap.x; odom.q[0] = att_pos_mocap.q[0];
mocap_odom.y = mocap.y; odom.q[1] = att_pos_mocap.q[1];
mocap_odom.z = mocap.z; odom.q[2] = att_pos_mocap.q[2];
mocap_odom.q[0] = mocap.q[0]; odom.q[3] = att_pos_mocap.q[3];
mocap_odom.q[1] = mocap.q[1];
mocap_odom.q[2] = mocap.q[2];
mocap_odom.q[3] = mocap.q[3];
const size_t URT_SIZE = sizeof(mocap_odom.pose_covariance) / sizeof(mocap_odom.pose_covariance[0]); odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
static_assert(URT_SIZE == (sizeof(mocap.covariance) / sizeof(mocap.covariance[0])), odom.position[0] = att_pos_mocap.x;
"Odometry Pose Covariance matrix URT array size mismatch"); odom.position[1] = att_pos_mocap.y;
odom.position[2] = att_pos_mocap.z;
for (size_t i = 0; i < URT_SIZE; i++) { // ATT_POS_MOCAP covariance
mocap_odom.pose_covariance[i] = mocap.covariance[i]; // 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; odom.orientation_variance[0] = att_pos_mocap.covariance[15]; // R row 3, col 3
mocap_odom.vx = NAN; odom.orientation_variance[1] = att_pos_mocap.covariance[18]; // P row 4, col 4
mocap_odom.vy = NAN; odom.orientation_variance[2] = att_pos_mocap.covariance[20]; // Y row 5, col 5
mocap_odom.vz = NAN;
mocap_odom.rollspeed = NAN;
mocap_odom.pitchspeed = NAN;
mocap_odom.yawspeed = NAN;
mocap_odom.velocity_covariance[0] = NAN;
_mocap_odometry_pub.publish(mocap_odom); odom.timestamp = hrt_absolute_time();
_mocap_odometry_pub.publish(odom);
} }
void void
@ -1313,143 +1328,259 @@ MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg)
void void
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
{ {
mavlink_vision_position_estimate_t ev; mavlink_vision_position_estimate_t vpe;
mavlink_msg_vision_position_estimate_decode(msg, &ev); 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(); odom.timestamp_sample = _mavlink_timesync.sync_stamp(vpe.usec);
visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec);
visual_odom.x = ev.x; odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
visual_odom.y = ev.y; odom.position[0] = vpe.x;
visual_odom.z = ev.z; odom.position[1] = vpe.y;
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw)); odom.position[2] = vpe.z;
q.copyTo(visual_odom.q);
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]); // VISION_POSITION_ESTIMATE covariance
static_assert(URT_SIZE == (sizeof(ev.covariance) / sizeof(ev.covariance[0])), // Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
"Odometry Pose Covariance matrix URT array size mismatch"); // (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++) { odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
visual_odom.pose_covariance[i] = ev.covariance[i]; 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; odom.reset_counter = vpe.reset_counter;
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;
visual_odom.reset_counter = ev.reset_counter; odom.timestamp = hrt_absolute_time();
_visual_odometry_pub.publish(visual_odom); _visual_odometry_pub.publish(odom);
} }
void void
MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
{ {
mavlink_odometry_t odom; mavlink_odometry_t odom_in;
mavlink_msg_odometry_decode(msg, &odom); 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(); odom.timestamp_sample = _mavlink_timesync.sync_stamp(odom_in.time_usec);
odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec);
/* The position is in a local FRD frame */ // position x/y/z (m)
odometry.x = odom.x; if (PX4_ISFINITE(odom_in.x) && PX4_ISFINITE(odom_in.y) && PX4_ISFINITE(odom_in.z)) {
odometry.y = odom.y; // frame_id: Coordinate frame of reference for the pose data.
odometry.z = odom.z; 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:
* The quaternion of the ODOMETRY msg represents a rotation from body frame // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
* to a local frame odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
*/ odom.position[0] = odom_in.y; // y: North
matrix::Quatf q_body_to_local(odom.q); odom.position[1] = odom_in.x; // x: East
q_body_to_local.normalize(); odom.position[2] = -odom_in.z; // z: Up
q_body_to_local.copyTo(odometry.q); 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 // pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]); // Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw)
static_assert(POS_URT_SIZE == (sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0])), // first six entries are the first ROW, next five entries are the second ROW, etc.
"Odometry Pose Covariance matrix URT array size mismatch"); 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;
// velocity_covariance case MAV_FRAME_LOCAL_ENU:
static constexpr size_t VEL_URT_SIZE = sizeof(odometry.velocity_covariance) / sizeof(odometry.velocity_covariance[0]); // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
static_assert(VEL_URT_SIZE == (sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0])), odom.position_variance[0] = odom_in.pose_covariance[6]; // Y row 1, col 1
"Odometry Velocity Covariance matrix URT array size mismatch"); 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;
// TODO: create a method to simplify covariance copy default:
for (size_t i = 0; i < POS_URT_SIZE; i++) { break;
odometry.pose_covariance[i] = odom.pose_covariance[i]; }
}
} }
/** // q: the quaternion of the ODOMETRY msg represents a rotation from body frame to a local frame
* PX4 expects the body's linear velocity in the local frame, if (PX4_ISFINITE(odom_in.q[0])
* the linear velocity is rotated from the odom child_frame to the && PX4_ISFINITE(odom_in.q[1])
* local NED frame. The angular velocity needs to be expressed in the && PX4_ISFINITE(odom_in.q[2])
* body (fcu_frd) frame. && PX4_ISFINITE(odom_in.q[3])) {
*/
if (odom.child_frame_id == MAV_FRAME_BODY_FRD) {
odometry.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD; odom.q[0] = odom_in.q[0];
odometry.vx = odom.vx; odom.q[1] = odom_in.q[1];
odometry.vy = odom.vy; odom.q[2] = odom_in.q[2];
odometry.vz = odom.vz; odom.q[3] = odom_in.q[3];
odometry.rollspeed = odom.rollspeed; // pose_covariance (roll, pitch, yaw)
odometry.pitchspeed = odom.pitchspeed; // states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.
odometry.yawspeed = odom.yawspeed; // TODO: fix pose_covariance for MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FLU
if (odom_in.estimator_type != MAV_ESTIMATOR_TYPE_NAIVE) {
for (size_t i = 0; i < VEL_URT_SIZE; i++) { odom.orientation_variance[0] = odom_in.pose_covariance[15]; // R row 3, col 3
odometry.velocity_covariance[i] = odom.velocity_covariance[i]; 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 { // velocity vx/vy/vz (m/s)
PX4_ERR("Body frame %" PRIu8 " not supported. Unable to publish velocity", odom.child_frame_id); 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: East
odom.velocity[1] = odom_in.vx; // x: North
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;
} }
odometry.reset_counter = odom.reset_counter; // 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:
* Supported local frame of reference is MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_FRD // ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth.
* The supported sources of the data/tesimator type are MAV_ESTIMATOR_TYPE_VISION, odom.velocity_variance[0] = odom_in.velocity_covariance[6]; // Y row 1, col 1
* MAV_ESTIMATOR_TYPE_VIO and MAV_ESTIMATOR_TYPE_MOCAP 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
* @note Regarding the local frames of reference, the appropriate EKF_AID_MASK break;
* 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) {
if (odom.frame_id == MAV_FRAME_LOCAL_NED) { default:
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; // unsupported child_frame_id
break;
} else { }
odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; }
} }
if ((odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION) // Roll/Pitch/Yaw angular speed (rad/s)
|| (odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO) if (PX4_ISFINITE(odom_in.rollspeed)
|| (odom.estimator_type == MAV_ESTIMATOR_TYPE_UNKNOWN)) { && PX4_ISFINITE(odom_in.pitchspeed)
// accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support && PX4_ISFINITE(odom_in.yawspeed)) {
_visual_odometry_pub.publish(odometry);
} else if (odom.estimator_type == MAV_ESTIMATOR_TYPE_MOCAP) { odom.angular_velocity[0] = odom_in.rollspeed;
_mocap_odometry_pub.publish(odometry); odom.angular_velocity[1] = odom_in.pitchspeed;
odom.angular_velocity[2] = odom_in.yawspeed;
} else {
PX4_ERR("Estimator source %" PRIu8 " not supported. Unable to publish pose and velocity", odom.estimator_type);
} }
} else { odom.reset_counter = odom_in.reset_counter;
PX4_ERR("Local frame %" PRIu8 " not supported. Unable to publish pose and velocity", odom.frame_id); 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

114
src/modules/mavlink/streams/ODOMETRY.hpp
View File

@ -74,93 +74,95 @@ private:
if (_mavlink->odometry_loopback_enabled()) { if (_mavlink->odometry_loopback_enabled()) {
odom_updated = _vodom_sub.update(&odom); 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 // source: external vision system
msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION; msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION;
} else { } else {
odom_updated = _odom_sub.update(&odom); odom_updated = _odom_sub.update(&odom);
msg.frame_id = MAV_FRAME_LOCAL_NED;
// source: PX4 estimator // source: PX4 estimator
msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT;
} }
if (odom_updated) { if (odom_updated) {
msg.time_usec = odom.timestamp_sample; msg.time_usec = odom.timestamp_sample;
// 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; msg.child_frame_id = MAV_FRAME_BODY_FRD;
break;
}
// Current position msg.x = odom.position[0];
msg.x = odom.x; msg.y = odom.position[1];
msg.y = odom.y; msg.z = odom.position[2];
msg.z = odom.z;
// Current orientation
msg.q[0] = odom.q[0]; msg.q[0] = odom.q[0];
msg.q[1] = odom.q[1]; msg.q[1] = odom.q[1];
msg.q[2] = odom.q[2]; msg.q[2] = odom.q[2];
msg.q[3] = odom.q[3]; msg.q[3] = odom.q[3];
switch (odom.velocity_frame) { msg.vx = odom.velocity[0];
case vehicle_odometry_s::BODY_FRAME_FRD: msg.vy = odom.velocity[1];
msg.vx = odom.vx; msg.vz = odom.velocity[2];
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;
}
// Current body rates // Current body rates
msg.rollspeed = odom.rollspeed; msg.rollspeed = odom.angular_velocity[0];
msg.pitchspeed = odom.pitchspeed; msg.pitchspeed = odom.angular_velocity[1];
msg.yawspeed = odom.yawspeed; msg.yawspeed = odom.angular_velocity[2];
// get the covariance matrix size
// pose_covariance // pose_covariance
static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]); // Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle
static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])), // (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.)
"Odometry Pose Covariance matrix URT array size mismatch"); 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 // velocity_covariance
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]); // Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle
static_assert(VEL_URT_SIZE == (sizeof(msg.velocity_covariance) / sizeof(msg.velocity_covariance[0])), // (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.)
"Odometry Velocity Covariance matrix URT array size mismatch"); for (auto &vc : msg.velocity_covariance) {
vc = NAN;
// copy pose covariances
for (size_t i = 0; i < POS_URT_SIZE; i++) {
msg.pose_covariance[i] = odom.pose_covariance[i];
} }
// copy velocity covariances msg.velocity_covariance[0] = odom.velocity_variance[0]; // X row 0, col 0
//TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame msg.velocity_covariance[6] = odom.velocity_variance[1]; // Y row 1, col 1
for (size_t i = 0; i < VEL_URT_SIZE; i++) { msg.velocity_covariance[11] = odom.velocity_variance[2]; // Z row 2, col 2
msg.velocity_covariance[i] = odom.velocity_covariance[i];
}
msg.reset_counter = odom.reset_counter; 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); mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg);
return true; return true;

1
src/modules/simulator/simulator.h
View File

@ -159,7 +159,6 @@ private:
void check_failure_injections(); void check_failure_injections();
int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
int publish_distance_topic(const mavlink_distance_sensor_t *dist); int publish_distance_topic(const mavlink_distance_sensor_t *dist);
static Simulator *_instance; 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; 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() Simulator::Simulator()
: ModuleParams(nullptr) : 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) 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) 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) 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) 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) int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink)
{ {
distance_sensor_s dist{}; distance_sensor_s dist{};