gz_navsat: integrate velocity on ellipsoid instead of projecting lpos

The issue is that the global position produced by gz is inconsistent
with the velocity, leading to navigation issues when moving away from
the origin.

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -718,19 +718,28 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &msg)
 	if (!_pos_ref.isInitialized()) {
 		_pos_ref.initReference(msg.latitude_deg(), msg.longitude_deg(), timestamp);
 		_alt_ref = msg.altitude();
+		_gpos = LatLonAlt(msg.latitude_deg(), msg.longitude_deg(), msg.altitude());
 		return;
 	}
 
-	double latitude = msg.latitude_deg();
-	double longitude = msg.longitude_deg();
-	double altitude = msg.altitude();
 	float vel_north = msg.velocity_north();
 	float vel_east = msg.velocity_east();
 	float vel_down = -msg.velocity_up();
 
-	vehicle_global_position_s gps_truth{};
+	const double dt = math::constrain((timestamp - _navsat_timestamp_prev) * 1e-6, 0.001, 0.1);
+	_navsat_timestamp_prev = timestamp;
+
+	// Velocity trapezoidal integration on the ellipsoid
+	const matrix::Vector3f vel_ned(vel_north, vel_east, vel_down);
+	_gpos += (vel_ned + _vel_ned_prev) * dt * 0.5f;
+	_vel_ned_prev = vel_ned;
+
+	double latitude = _gpos.latitude_deg();
+	double longitude = _gpos.longitude_deg();
+	double altitude = _gpos.altitude();
 
 	// Publish GPS groundtruth
+	vehicle_global_position_s gps_truth{};
 	gps_truth.timestamp = timestamp;
 	gps_truth.timestamp_sample = timestamp;
 	gps_truth.lat = latitude;

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -46,6 +46,7 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <lib/drivers/device/Device.hpp>
 #include <lib/geo/geo.h>
+#include <lib/lat_lon_alt/lat_lon_alt.hpp>
 
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
@@ -164,7 +165,10 @@ private:
 	GZGimbal _gimbal{_node};
 
 	MapProjection _pos_ref{};
+	LatLonAlt _gpos{0.0, 0.0, 0.f};
 	double _alt_ref{};
+	matrix::Vector3f _vel_ned_prev{};
+	hrt_abstime _navsat_timestamp_prev{};
 
 	matrix::Vector3d _position_prev{};
 	matrix::Vector3d _velocity_prev{};