fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 07:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support odometry velocity in body and local frame (#14703)

Browse Source 
* Update submodule ECL

* increase lower bound on EVV param
This commit is contained in:
kritz
2020-05-13 12:43:02 +02:00
committed by GitHub
parent 8804dae480
commit 3897030c6f
8 changed files with 86 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+19 -8
View File
@@ -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;
src/modules/mavlink/mavlink_receiver.cpp
+9 -27
View File
@@ -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<float> linvel_local(R_body_to_local * matrix::Vector3<float>(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);
}