diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 7f2f023eed..bd100a4eee 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -21,6 +21,7 @@ uint8 COVARIANCE_MATRIX_YAWRATE_VARIANCE=20 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 # Position and linear velocity local frame of reference uint8 local_frame @@ -41,7 +42,10 @@ float32[4] q_offset # Quaternion rotation from odometry reference frame to navi # If orientation covariance invalid/unknown, 16th cell is NaN float32[21] pose_covariance -# Velocity in meters/sec. Frame of reference defined by local_frame. NaN if invalid/unknown +# 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 diff --git a/src/lib/ecl b/src/lib/ecl index 03191847f9..24f2e60b7e 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 03191847f9c162dedc2cb45ff775b75cbc9c8b36 +Subproject commit 24f2e60b7eff664d0035d5f75f410073be28d894 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 37356f158d..a2cdd137b2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1117,18 +1117,28 @@ void Ekf2::Run() 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) { + ev_data.vel_frame = estimator::BODY_FRAME_FRD; + + } else { + ev_data.vel_frame = estimator::LOCAL_FRAME_FRD; + } + // velocity measurement error from ev_data or parameters float param_evv_noise_var = sq(_param_ekf2_evv_noise.get()); 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.velVar(0) = fmaxf(param_evv_noise_var, _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]); - ev_data.velVar(1) = fmaxf(param_evv_noise_var, _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]); - ev_data.velVar(2) = fmaxf(param_evv_noise_var, _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]; } else { - ev_data.velVar.setAll(param_evv_noise_var); + ev_data.velCov = matrix::eye() * param_evv_noise_var; } } @@ -1234,7 +1244,7 @@ void Ekf2::Run() odom.timestamp = hrt_absolute_time(); odom.timestamp_sample = now; - odom.local_frame = odom.LOCAL_FRAME_NED; + odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; // Position of body origin in local NED frame Vector3f position = _ekf.getPosition(); @@ -1256,6 +1266,7 @@ void Ekf2::Run() lpos.vz = velocity(2); // Vehicle odometry linear velocity + odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; odom.vx = lpos.vx; odom.vy = lpos.vy; odom.vz = lpos.vz; @@ -1417,10 +1428,27 @@ void Ekf2::Run() aligned_ev_odom.y = aligned_pos(1); aligned_ev_odom.z = aligned_pos(2); - 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); + 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); + 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); + break; + } + } + + aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED; // Compute orientation in EKF navigation frame Quatf ev_quat_aligned = quat_ev2ekf * matrix::Quatf(_ev_odom.q) ; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 8dff2d1f87..5d9eff3509 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -750,7 +750,7 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f); * Measurement noise for vision angle observations used to lower bound or replace the uncertainty included in the message * * @group EKF2 - * @min 0.01 + * @min 0.05 * @unit rad * @decimal 2 */ diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index cba8183420..0c589016f2 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -638,7 +638,7 @@ void BlockLocalPositionEstimator::publishOdom() && PX4_ISFINITE(_x(X_vz))) { _pub_odom.get().timestamp = hrt_absolute_time(); _pub_odom.get().timestamp_sample = _timeStamp; - _pub_odom.get().local_frame = _pub_odom.get().LOCAL_FRAME_NED; + _pub_odom.get().local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; // position _pub_odom.get().x = xLP(X_x); // north @@ -656,6 +656,7 @@ 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 diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5a4c36e532..ce41e652c2 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2572,16 +2572,27 @@ protected: msg.q[2] = odom.q[2]; msg.q[3] = odom.q[3]; - // Body-FRD frame to local NED frame Dcm matrix - matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q)); + switch (odom.velocity_frame) { + case vehicle_odometry_s::BODY_FRAME_FRD: + msg.vx = odom.vx; + msg.vy = odom.vy; + msg.vz = odom.vz; + break; - // Rotate linear and angular velocity from local NED to body-NED frame - matrix::Vector3f linvel_body(R_body_to_local.transpose() * matrix::Vector3f(odom.vx, odom.vy, odom.vz)); + 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)); - // Current linear velocity - msg.vx = linvel_body(0); - msg.vy = linvel_body(1); - msg.vz = linvel_body(2); + // 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 msg.rollspeed = odom.rollspeed; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9a3db19570..ef4f675f90 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -792,6 +792,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) mocap_odom.pose_covariance[i] = mocap.covariance[i]; } + mocap_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; mocap_odom.vx = NAN; mocap_odom.vy = NAN; mocap_odom.vz = NAN; @@ -1266,7 +1267,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) // - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define // a frame of reference which is not aligned with NED or ENU // - add usage on the estimator side - visual_odom.local_frame = visual_odom.LOCAL_FRAME_NED; + visual_odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED; 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])), @@ -1276,6 +1277,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) visual_odom.pose_covariance[i] = ev.covariance[i]; } + visual_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; visual_odom.vx = NAN; visual_odom.vy = NAN; visual_odom.vz = NAN; @@ -1312,7 +1314,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) // TODO: // add usage of this information on the estimator side // The heading of the local frame is not aligned with north - odometry.local_frame = odometry.LOCAL_FRAME_FRD; + odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; // pose_covariance static constexpr size_t POS_URT_SIZE = sizeof(odometry.pose_covariance) / sizeof(odometry.pose_covariance[0]); @@ -1336,40 +1338,20 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) * body (fcu_frd) frame. */ if (odom.child_frame_id == MAV_FRAME_BODY_FRD) { - /* Linear velocity has to be rotated to the local NED frame - * Angular velocities are already in the right body frame */ - const matrix::Dcmf R_body_to_local = matrix::Dcmf(q_body_to_local); - /* the linear velocities needs to be transformed to the local frame FRD*/ - matrix::Vector3 linvel_local(R_body_to_local * matrix::Vector3(odom.vx, odom.vy, odom.vz)); - - odometry.vx = linvel_local(0); - odometry.vy = linvel_local(1); - odometry.vz = linvel_local(2); + odometry.velocity_frame = vehicle_odometry_s::BODY_FRAME_FRD; + odometry.vx = odom.vx; + odometry.vy = odom.vy; + odometry.vz = odom.vz; odometry.rollspeed = odom.rollspeed; odometry.pitchspeed = odom.pitchspeed; odometry.yawspeed = odom.yawspeed; - /* the linear velocity's covariance needs to be transformed to the local frame FRD*/ - matrix::Matrix3f lin_vel_cov_body; - lin_vel_cov_body(0, 0) = odom.velocity_covariance[odometry.COVARIANCE_MATRIX_VX_VARIANCE]; - lin_vel_cov_body(0, 1) = lin_vel_cov_body(1, 0) = odom.velocity_covariance[1]; - lin_vel_cov_body(0, 2) = lin_vel_cov_body(2, 0) = odom.velocity_covariance[2]; - lin_vel_cov_body(1, 1) = odom.velocity_covariance[odometry.COVARIANCE_MATRIX_VY_VARIANCE]; - lin_vel_cov_body(1, 2) = lin_vel_cov_body(2, 1) = odom.velocity_covariance[7]; - lin_vel_cov_body(2, 2) = odom.velocity_covariance[odometry.COVARIANCE_MATRIX_VZ_VARIANCE]; - matrix::Matrix3f lin_vel_cov_local = R_body_to_local * lin_vel_cov_body * R_body_to_local.transpose(); - - /* Only the linear velocity variance elements are used */ for (size_t i = 0; i < VEL_URT_SIZE; i++) { - odometry.velocity_covariance[i] = NAN; + odometry.velocity_covariance[i] = odom.velocity_covariance[i]; } - odometry.velocity_covariance[odometry.COVARIANCE_MATRIX_VX_VARIANCE] = lin_vel_cov_local(0, 0); - odometry.velocity_covariance[odometry.COVARIANCE_MATRIX_VY_VARIANCE] = lin_vel_cov_local(1, 1); - odometry.velocity_covariance[odometry.COVARIANCE_MATRIX_VZ_VARIANCE] = lin_vel_cov_local(2, 2); - } else { PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id); } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index dc9a80002e..5499bdd4a6 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1027,24 +1027,17 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) mavlink_odometry_t odom_msg; mavlink_msg_odometry_decode(odom_mavlink, &odom_msg); - /* The quaternion of the ODOMETRY msg represents a rotation from - * FRD body frame to the NED local frame */ - matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]); - q.normalize(); - q.copyTo(odom.q); - - /* Dcm rotation matrix from body frame to local NED frame */ - const matrix::Dcmf R_body_to_local(q); - - /* since odom.child_frame_id == MAV_FRAME_BODY_FRD, WRT to estimated vehicle body-fixed frame */ - /* No need to transform the covariance matrices since the non-diagonal values are all zero */ - /* The position in the local NED frame */ odom.x = odom_msg.x; odom.y = odom_msg.y; odom.z = odom_msg.z; - odom.local_frame = odom.LOCAL_FRAME_FRD; + /* 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); + + 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"); @@ -1054,14 +1047,12 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) odom.pose_covariance[i] = odom_msg.pose_covariance[i]; } - /* the linear velocities needs to be transformed from the body frame to the local frame */ - const matrix::Vector3 linvel_local(R_body_to_local * - matrix::Vector3(odom_msg.vx, odom_msg.vy, odom_msg.vz)); + /* 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 velocity in the local NED frame */ - odom.vx = linvel_local(0); - odom.vy = linvel_local(1); - odom.vz = linvel_local(2); /* The angular velocity in the body-fixed frame */ odom.rollspeed = odom_msg.rollspeed; odom.pitchspeed = odom_msg.pitchspeed; @@ -1089,7 +1080,7 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw)); q.copyTo(odom.q); - odom.local_frame = odom.LOCAL_FRAME_NED; + 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");