EKF2: Always publish global position

For historical reasons, there is a check in EKF2 to only publish global
position if it has moved for 1mm.

This is no longer necessary, and also this if doesn't save any cpu cycles
in real conditions where GPS errors are always much bigger that this.

When using simulated or fake GPS, based on some other positioning
system, the GPS coordinate input can be very accurate due to quantization
or true accuracy. In this case this check bites and perfectly valid position
doesn't get published.

Just removed this if.

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

src/modules/ekf2/EKF2.cpp

@@ -628,10 +628,8 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 {
 	if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {
-		// only publish if position has changed by at least 1 mm (map_projection_reproject is relatively expensive)
 		const Vector3f position{_ekf.getPosition()};
 
-		if ((_last_local_position_for_gpos - position).longerThan(0.001f)) {
 		// generate and publish global position data
 		vehicle_global_position_s global_pos;
 		global_pos.timestamp_sample = timestamp;
@@ -666,9 +664,6 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 		global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
 		global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_global_position_pub.publish(global_pos);
-
-			_last_local_position_for_gpos = position;
-		}
 	}
 }
 

src/modules/ekf2/EKF2.hpp

@@ -212,8 +212,6 @@ private:
 	uint32_t _device_id_gyro{0};
 	uint32_t _device_id_mag{0};
 
-	Vector3f _last_local_position_for_gpos{};
-
 	Vector3f _last_accel_bias_published{};
 	Vector3f _last_gyro_bias_published{};
 	Vector3f _last_mag_bias_published{};