Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

EKF2: move vehicle_odometry publish to method

Browse Source
This commit is contained in:
Daniel Agar
2020-09-28 18:34:49 -04:00
committed by GitHub
parent faccb0d948
commit f1feaf45d3
2 changed files with 68 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+67 -61
View File
@@ -685,12 +685,6 @@ void EKF2::Run()
vehicle_local_position_s &lpos = _vehicle_local_position_pub.get();
lpos.timestamp_sample = imu_sample_new.time_us;
// generate vehicle odometry data
vehicle_odometry_s odom{};
odom.timestamp_sample = imu_sample_new.time_us;
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// Position of body origin in local NED frame
Vector3f position = _ekf.getPosition();
const float lpos_x_prev = lpos.x;
@@ -699,23 +693,12 @@ void EKF2::Run()
lpos.y = (_ekf.local_position_is_valid()) ? position(1) : 0.0f;
lpos.z = position(2);
// Vehicle odometry position
odom.x = lpos.x;
odom.y = lpos.y;
odom.z = lpos.z;
// Velocity of body origin in local NED frame (m/s)
const Vector3f velocity = _ekf.getVelocity();
lpos.vx = velocity(0);
lpos.vy = velocity(1);
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;
// vertical position time derivative (m/s)
lpos.z_deriv = _ekf.getVerticalPositionDerivative();
@@ -755,16 +738,6 @@ void EKF2::Run()
lpos.heading = matrix::Eulerf(q).psi();
lpos.delta_heading = matrix::Eulerf(delta_q_reset).psi();
// Vehicle odometry quaternion
q.copyTo(odom.q);
// Vehicle odometry angular rates
const Vector3f gyro_bias = _ekf.getGyroBias();
const Vector3f rates(imu_sample_new.delta_ang * imu_sample_new.delta_ang_dt);
odom.rollspeed = rates(0) - gyro_bias(0);
odom.pitchspeed = rates(1) - gyro_bias(1);
odom.yawspeed = rates(2) - gyro_bias(2);
lpos.dist_bottom_valid = _ekf.get_terrain_valid();
float terrain_vpos = _ekf.getTerrainVertPos();
@@ -827,44 +800,12 @@ void EKF2::Run()
lpos.hagl_max = INFINITY;
}
// Get covariances to vehicle odometry
float covariances[24];
_ekf.covariances_diagonal().copyTo(covariances);
// get the covariance matrix size
const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
// initially set pose covariances to 0
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = 0.0;
}
// 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];
// publish vehicle local position data
lpos.timestamp = _replay_mode ? now : hrt_absolute_time();
_vehicle_local_position_pub.update();
// publish vehicle odometry data
odom.timestamp = _replay_mode ? now : hrt_absolute_time();
_vehicle_odometry_pub.publish(odom);
// publish vehicle_odometry
publish_odometry(now, imu_sample_new, lpos);
// publish external visual odometry after fixed frame alignment if new odometry is received
if (new_ev_data_received) {
@@ -1286,6 +1227,71 @@ void EKF2::publish_attitude(const hrt_abstime &timestamp)
}
}
void EKF2::publish_odometry(const hrt_abstime &timestamp, const imuSample &imu, const vehicle_local_position_s &lpos)
{
// generate vehicle odometry data
vehicle_odometry_s odom{};
odom.timestamp_sample = imu.time_us;
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// Vehicle odometry position
odom.x = lpos.x;
odom.y = lpos.y;
odom.z = lpos.z;
// 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;
// Vehicle odometry 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);
// get the covariance matrix size
const size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
const size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
// Get covariances to vehicle odometry
float covariances[24];
_ekf.covariances_diagonal().copyTo(covariances);
// initially set pose covariances to 0
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = 0.0;
}
// 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];
// publish vehicle odometry data
odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_vehicle_odometry_pub.publish(odom);
}
void EKF2::publish_yaw_estimator_status(const hrt_abstime &timestamp)
{
yaw_estimator_status_s yaw_est_test_data{};
src/modules/ekf2/EKF2.hpp
+1
View File
@@ -122,6 +122,7 @@ private:
bool update_mag_decl(Param &mag_decl_param);
void publish_attitude(const hrt_abstime &timestamp);
void publish_odometry(const hrt_abstime &timestamp, const imuSample &imu, const vehicle_local_position_s &lpos);
void publish_wind_estimate(const hrt_abstime &timestamp);
void publish_yaw_estimator_status(const hrt_abstime &timestamp);