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

ekf2: move ekf_gps_drift publication to method

Browse Source
This commit is contained in:
Daniel Agar
2020-11-10 10:02:49 -05:00
parent 0d657c74bf
commit e397cbf8bb
2 changed files with 22 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+21 -15
View File
@@ -923,21 +923,6 @@ void EKF2::Run()
_estimator_sensor_bias_pub.publish(bias);
}
// publish GPS drift data only when updated to minimise overhead
float gps_drift[3];
bool blocked;
if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
ekf_gps_drift_s drift_data;
drift_data.hpos_drift_rate = gps_drift[0];
drift_data.vpos_drift_rate = gps_drift[1];
drift_data.hspd = gps_drift[2];
drift_data.blocked = blocked;
drift_data.timestamp = _replay_mode ? now : hrt_absolute_time();
_ekf_gps_drift_pub.publish(drift_data);
}
{
/* Check and save learned magnetometer bias estimates */
@@ -1010,6 +995,9 @@ void EKF2::Run()
PublishWindEstimate(now);
PublishYawEstimatorStatus(now);
// publish status/logging messages
PublishEkfDriftMetrics(now);
if (!_mag_decl_saved && _standby) {
_mag_decl_saved = update_mag_decl(_param_ekf2_mag_decl);
}
@@ -1239,6 +1227,24 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
}
}
void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
{
// publish GPS drift data only when updated to minimise overhead
float gps_drift[3];
bool blocked;
if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
ekf_gps_drift_s drift_data;
drift_data.hpos_drift_rate = gps_drift[0];
drift_data.vpos_drift_rate = gps_drift[1];
drift_data.hspd = gps_drift[2];
drift_data.blocked = blocked;
drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_ekf_gps_drift_pub.publish(drift_data);
}
}
void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
{
// generate vehicle odometry data