diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 27cfe93c4d..4bd8c5ca30 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -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 diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 627bd89121..ad5db582e4 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -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); diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 4d7f66cf68..0a0ecbf57b 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -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{}; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 9532178b58..8898d98eba 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -201,6 +201,8 @@ public: // get the orientation (quaterion) covariances matrix::SquareMatrix orientation_covariances() const { return P.slice<4, 4>(0, 0); } + matrix::SquareMatrix orientation_covariances_euler() const; + // get the linear velocity covariances matrix::SquareMatrix velocity_covariances() const { return P.slice<3, 3>(4, 4); } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 21fb9730e5..334d95345b 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -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 Ekf::orientation_covariances_euler() const +{ + // Jacobian matrix (3x4) containing the partial derivatives of the + // Euler angle equations with respect to the quaternions + matrix::Matrix 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 quat_covariances = P.slice<4, 4>(0, 0); + + return G * quat_covariances * G.transpose(); +} diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index a4cc1507fb..ed99a2f5f4 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -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; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 0987281f46..625ce125ad 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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 ×tamp) _local_position_pub.publish(lpos); } -void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu) +void EKF2::PublishOdometry(const hrt_abstime ×tamp) { // 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 ×tamp, 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() * 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); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 59f6d66565..4c94bfd24f 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -144,7 +144,7 @@ private: void PublishInnovationTestRatios(const hrt_abstime ×tamp); void PublishInnovationVariances(const hrt_abstime ×tamp); void PublishLocalPosition(const hrt_abstime ×tamp); - void PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu); + void PublishOdometry(const hrt_abstime ×tamp); void PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom); void PublishOpticalFlowVel(const hrt_abstime ×tamp); void PublishSensorBias(const hrt_abstime ×tamp); diff --git a/src/modules/ekf2/test/sensor_simulator/vio.cpp b/src/modules/ekf2/test/sensor_simulator/vio.cpp index 25df31253a..42723306e0 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.cpp +++ b/src/modules/ekf2/test/sensor_simulator/vio.cpp @@ -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() * 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; diff --git a/src/modules/ekf2/test/sensor_simulator/vio.h b/src/modules/ekf2/test/sensor_simulator/vio.h index d37364d651..2000865e22 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.h +++ b/src/modules/ekf2/test/sensor_simulator/vio.h @@ -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); diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index c641baa201..3fbde311a6 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -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(); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 1668db9ca8..bf85965c23 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -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(); diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index 8a441146c6..b099873500 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -62,15 +62,10 @@ void BlockLocalPositionEstimator::mocapInit() int BlockLocalPositionEstimator::mocapMeasure(Vector &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 &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; diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index b9e5340303..bbbeb4e33d 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -67,15 +67,10 @@ void BlockLocalPositionEstimator::visionInit() int BlockLocalPositionEstimator::visionMeasure(Vector &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 &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; diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 05864e218e..c46af52326 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 05864e218e204f1ebeee5555988150fcddbd873e +Subproject commit c46af523263ff0dad59afe018fe387c1df030442 diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4265311f9a..96bc7a83d7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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(), #endif // VIBRATION_HPP -#if defined(ATT_POS_MOCAP_HPP) - create_stream_list_item(), -#endif // ATT_POS_MOCAP_HPP #if defined(AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP) create_stream_list_item(), #endif // AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c7060cb565..42257a2ffa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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(events::ID("mavlink_rcv_odom_unsup_estimator_type"), events::Log::Error, + "ODOMETRY: unsupported estimator_type {1}", odom_in.estimator_type); + return; } } diff --git a/src/modules/mavlink/streams/ATT_POS_MOCAP.hpp b/src/modules/mavlink/streams/ATT_POS_MOCAP.hpp deleted file mode 100644 index 07ea7a9129..0000000000 --- a/src/modules/mavlink/streams/ATT_POS_MOCAP.hpp +++ /dev/null @@ -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 - -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 diff --git a/src/modules/mavlink/streams/ODOMETRY.hpp b/src/modules/mavlink/streams/ODOMETRY.hpp index 2a0e148541..f423b16bfb 100644 --- a/src/modules/mavlink/streams/ODOMETRY.hpp +++ b/src/modules/mavlink/streams/ODOMETRY.hpp @@ -74,93 +74,95 @@ private: if (_mavlink->odometry_loopback_enabled()) { odom_updated = _vodom_sub.update(&odom); - // set the frame_id according to the local frame of the data - if (odom.local_frame == vehicle_odometry_s::LOCAL_FRAME_NED) { - msg.frame_id = MAV_FRAME_LOCAL_NED; - - } else { - msg.frame_id = MAV_FRAME_LOCAL_FRD; - } - // source: external vision system msg.estimator_type = MAV_ESTIMATOR_TYPE_VISION; } else { odom_updated = _odom_sub.update(&odom); - msg.frame_id = MAV_FRAME_LOCAL_NED; - // source: PX4 estimator msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; } if (odom_updated) { msg.time_usec = odom.timestamp_sample; - msg.child_frame_id = MAV_FRAME_BODY_FRD; - // Current position - msg.x = odom.x; - msg.y = odom.y; - msg.z = odom.z; + // set the frame_id according to the local frame of the data + switch (odom.pose_frame) { + case vehicle_odometry_s::POSE_FRAME_NED: + msg.frame_id = MAV_FRAME_LOCAL_NED; + break; + + case vehicle_odometry_s::POSE_FRAME_FRD: + msg.frame_id = MAV_FRAME_LOCAL_FRD; + break; + } + + switch (odom.velocity_frame) { + case vehicle_odometry_s::VELOCITY_FRAME_NED: + msg.child_frame_id = MAV_FRAME_LOCAL_NED; + break; + + case vehicle_odometry_s::VELOCITY_FRAME_FRD: + msg.child_frame_id = MAV_FRAME_LOCAL_FRD; + break; + + case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: + msg.child_frame_id = MAV_FRAME_BODY_FRD; + break; + } + + msg.x = odom.position[0]; + msg.y = odom.position[1]; + msg.z = odom.position[2]; - // Current orientation msg.q[0] = odom.q[0]; msg.q[1] = odom.q[1]; msg.q[2] = odom.q[2]; msg.q[3] = odom.q[3]; - switch (odom.velocity_frame) { - case vehicle_odometry_s::BODY_FRAME_FRD: - msg.vx = odom.vx; - msg.vy = odom.vy; - msg.vz = odom.vz; - break; - - case vehicle_odometry_s::LOCAL_FRAME_FRD: - case vehicle_odometry_s::LOCAL_FRAME_NED: - // Body frame to local frame - const matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q)); - - // Rotate linear velocity from local to body frame - const matrix::Vector3f linvel_body(R_body_to_local.transpose() * - matrix::Vector3f(odom.vx, odom.vy, odom.vz)); - - msg.vx = linvel_body(0); - msg.vy = linvel_body(1); - msg.vz = linvel_body(2); - break; - } + msg.vx = odom.velocity[0]; + msg.vy = odom.velocity[1]; + msg.vz = odom.velocity[2]; // Current body rates - msg.rollspeed = odom.rollspeed; - msg.pitchspeed = odom.pitchspeed; - msg.yawspeed = odom.yawspeed; - - // get the covariance matrix size + msg.rollspeed = odom.angular_velocity[0]; + msg.pitchspeed = odom.angular_velocity[1]; + msg.yawspeed = odom.angular_velocity[2]; // pose_covariance - static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]); - static_assert(POS_URT_SIZE == (sizeof(msg.pose_covariance) / sizeof(msg.pose_covariance[0])), - "Odometry Pose Covariance matrix URT array size mismatch"); + // Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle + // (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.) + for (auto &pc : msg.pose_covariance) { + pc = NAN; + } + + msg.pose_covariance[0] = odom.position_variance[0]; // X row 0, col 0 + msg.pose_covariance[6] = odom.position_variance[1]; // Y row 1, col 1 + msg.pose_covariance[11] = odom.position_variance[2]; // Z row 2, col 2 + + msg.pose_covariance[15] = odom.orientation_variance[0]; // R row 3, col 3 + msg.pose_covariance[18] = odom.orientation_variance[1]; // P row 4, col 4 + msg.pose_covariance[20] = odom.orientation_variance[2]; // Y row 5, col 5 // velocity_covariance - static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]); - static_assert(VEL_URT_SIZE == (sizeof(msg.velocity_covariance) / sizeof(msg.velocity_covariance[0])), - "Odometry Velocity Covariance matrix URT array size mismatch"); - - // copy pose covariances - for (size_t i = 0; i < POS_URT_SIZE; i++) { - msg.pose_covariance[i] = odom.pose_covariance[i]; + // Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle + // (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.) + for (auto &vc : msg.velocity_covariance) { + vc = NAN; } - // copy velocity covariances - //TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame - for (size_t i = 0; i < VEL_URT_SIZE; i++) { - msg.velocity_covariance[i] = odom.velocity_covariance[i]; - } + msg.velocity_covariance[0] = odom.velocity_variance[0]; // X row 0, col 0 + msg.velocity_covariance[6] = odom.velocity_variance[1]; // Y row 1, col 1 + msg.velocity_covariance[11] = odom.velocity_variance[2]; // Z row 2, col 2 msg.reset_counter = odom.reset_counter; + // source: PX4 estimator + msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; + + msg.quality = odom.quality; + mavlink_msg_odometry_send_struct(_mavlink->get_channel(), &msg); return true; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 951e33e83e..e696b999e7 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -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; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d3d749c913..962760c898 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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{};