Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

ekf2: publish flow vel only if compensated flow is available

Browse Source 
- fix a few publication timestamp_samples
This commit is contained in:
Daniel Agar
2022-02-13 13:57:01 -05:00
parent b157afde6a
commit 5370733d62
3 changed files with 41 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+35 -30
View File
@@ -552,6 +552,7 @@ void EKF2::Run()
UpdateAirspeedSample(ekf2_timestamps);
UpdateAuxVelSample(ekf2_timestamps);
UpdateBaroSample(ekf2_timestamps);
UpdateFlowSample(ekf2_timestamps);
UpdateGpsSample(ekf2_timestamps);
UpdateMagSample(ekf2_timestamps);
UpdateRangeSample(ekf2_timestamps);
@@ -559,10 +560,6 @@ void EKF2::Run()
vehicle_odometry_s ev_odom;
const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
optical_flow_s optical_flow;
const bool new_optical_flow = UpdateFlowSample(ekf2_timestamps, optical_flow);
// run the EKF update and output
const hrt_abstime ekf_update_start = hrt_absolute_time();
@@ -578,12 +575,13 @@ void EKF2::Run()
PublishBaroBias(now);
PublishEkfDriftMetrics(now);
PublishEventFlags(now);
PublishInnovations(now);
PublishInnovationTestRatios(now);
PublishInnovationVariances(now);
PublishOpticalFlowVel(now);
PublishStates(now);
PublishStatus(now);
PublishStatusFlags(now);
PublishInnovations(now, imu_sample_new);
PublishInnovationTestRatios(now);
PublishInnovationVariances(now);
PublishYawEstimatorStatus(now);
UpdateAccelCalibration(now);
@@ -601,10 +599,6 @@ void EKF2::Run()
PublishOdometryAligned(now, ev_odom);
}
if (new_optical_flow) {
PublishOpticalFlowVel(now, optical_flow);
}
// publish ekf2_timestamps
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
@@ -638,7 +632,7 @@ void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) {
estimator_baro_bias_s baro_bias{};
baro_bias.timestamp_sample = timestamp;
baro_bias.timestamp_sample = _ekf.get_baro_sample_delayed().time_us;
baro_bias.baro_device_id = _device_id_baro;
baro_bias.bias = status.bias;
baro_bias.bias_var = status.bias_var;
@@ -774,7 +768,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
}
}
void EKF2::PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu)
void EKF2::PublishInnovations(const hrt_abstime &timestamp)
{
// publish estimator innovation data
estimator_innovations_s innovations{};
@@ -805,7 +799,7 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
_preflt_checker.update(imu.delta_ang_dt, innovations);
_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
}
}
@@ -974,7 +968,7 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
{
// generate vehicle odometry data
vehicle_odometry_s odom;
odom.timestamp_sample = imu.time_us;
odom.timestamp_sample = timestamp;
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
@@ -1081,6 +1075,7 @@ void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_od
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);
}
@@ -1098,7 +1093,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
|| (timestamp >= _last_sensor_bias_published + 1_s)) {
estimator_sensor_bias_s bias{};
bias.timestamp_sample = timestamp;
bias.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
// take device ids from sensor_selection_s if not using specific vehicle_imu_s
if (_device_id_gyro != 0) {
@@ -1318,7 +1313,7 @@ void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
yaw_est_test_data.innov_vn, yaw_est_test_data.innov_ve,
yaw_est_test_data.weight)) {
yaw_est_test_data.timestamp_sample = timestamp;
yaw_est_test_data.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_yaw_est_pub.publish(yaw_est_test_data);
@@ -1349,19 +1344,21 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
}
}
void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &flow_sample)
void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
{
estimator_optical_flow_vel_s flow_vel{};
flow_vel.timestamp_sample = flow_sample.timestamp;
if (_ekf.getFlowCompensated().longerThan(0.f)) {
estimator_optical_flow_vel_s flow_vel{};
flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_optical_flow_vel_pub.publish(flow_vel);
_estimator_optical_flow_vel_pub.publish(flow_vel);
}
}
float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
@@ -1551,6 +1548,8 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
} else {
ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
}
new_ev_odom = true;
}
// check for valid position data
@@ -1572,6 +1571,8 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
} else {
ev_data.posVar.setAll(param_evp_noise_var);
}
new_ev_odom = true;
}
// check for valid orientation data
@@ -1587,13 +1588,16 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
} else {
ev_data.angVar = param_eva_noise_var;
}
new_ev_odom = true;
}
// use timestamp from external computer, clocks are synchronized when using MAVROS
ev_data.time_us = ev_odom.timestamp_sample;
_ekf.setExtVisionData(ev_data);
new_ev_odom = true;
if (new_ev_odom) {
_ekf.setExtVisionData(ev_data);
}
ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
@@ -1602,11 +1606,12 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
return new_ev_odom;
}
bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow)
bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF flow sample
bool new_optical_flow = false;
const unsigned last_generation = _optical_flow_sub.get_last_generation();
optical_flow_s optical_flow;
if (_optical_flow_sub.update(&optical_flow)) {
if (_msg_missed_optical_flow_perf == nullptr) {