ekf2: publish flow vel only if compensated flow is available

- fix a few publication timestamp_samples

src/modules/ekf2/EKF/control.cpp

@@ -410,7 +410,10 @@ void Ekf::controlOpticalFlowFusion()
 		} else {
 			// don't use this flow data and wait for the next data to arrive
 			_flow_data_ready = false;
+			_flow_compensated_XY_rad.setZero();
 		}
+	} else {
+		_flow_compensated_XY_rad.setZero();
 	}
 
 	// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon

src/modules/ekf2/EKF2.cpp

@@ -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{};
+	if (_ekf.getFlowCompensated().longerThan(0.f)) {
-	flow_vel.timestamp_sample = flow_sample.timestamp;
+		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.getFlowVelBody().copyTo(flow_vel.vel_body);
-	_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
+		_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
-	_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
+		_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
-	_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
+		_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
-	flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		_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) {

src/modules/ekf2/EKF2.hpp

@@ -140,13 +140,13 @@ private:
 	void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
 	void PublishEventFlags(const hrt_abstime &timestamp);
 	void PublishGlobalPosition(const hrt_abstime &timestamp);
-	void PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu);
+	void PublishInnovations(const hrt_abstime &timestamp);
 	void PublishInnovationTestRatios(const hrt_abstime &timestamp);
 	void PublishInnovationVariances(const hrt_abstime &timestamp);
 	void PublishLocalPosition(const hrt_abstime &timestamp);
 	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu);
 	void PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom);
-	void PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &optical_flow);
+	void PublishOpticalFlowVel(const hrt_abstime &timestamp);
 	void PublishSensorBias(const hrt_abstime &timestamp);
 	void PublishStates(const hrt_abstime &timestamp);
 	void PublishStatus(const hrt_abstime &timestamp);
@@ -158,7 +158,7 @@ private:
 	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
 	bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom);
-	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow);
+	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);