SensorGps.msg: switch to double precision for lat/lon/alt

To match PX4/PX4-GPSDrivers#132 - adding high precision RTK lat/lon/alt components

Author: slgrobotics

msg/SensorGps.msg

@@ -5,10 +5,10 @@ uint64 timestamp_sample
 
 uint32 device_id                # unique device ID for the sensor that does not change between power cycles
 
-int32 lat			# Latitude in 1E-7 degrees
-int32 lon			# Longitude in 1E-7 degrees
-int32 alt			# Altitude in 1E-3 meters above MSL, (millimetres)
-int32 alt_ellipsoid 		# Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
+float64 latitude_deg		# Latitude in degrees, allows centimeter level RTK precision
+float64 longitude_deg		# Longitude in degrees, allows centimeter level RTK precision
+float64 altitude_msl_m		# Altitude above MSL, meters
+float64 altitude_ellipsoid_m	# Altitude above Ellipsoid, meters
 
 float32 s_variance_m_s		# GPS speed accuracy estimate, (metres/sec)
 float32 c_variance_rad		# GPS course accuracy estimate, (radians)

src/drivers/cyphal/Publishers/udral/Gnss.hpp

@@ -66,9 +66,9 @@ class UavcanGnssPublisher : public UavcanPublisher
 			size_t payload_size = reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
 
 			reg_udral_physics_kinematics_geodetic_Point_0_1 geo {};
-			geo.latitude = gps.lat;
-			geo.longitude = gps.lon;
-			geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast<double>(gps.alt) };
+			geo.latitude = (int64_t)(gps.latitude_deg / 1e7);
+			geo.longitude = (int64_t)(gps.longitude_deg / 1e7);
+			geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = gps.altitude_msl_m };
 
 			uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
 

src/drivers/gps/devices

@@ -320,10 +320,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 
 			sensor_gps.fix_type = gpsFix;
 
-			sensor_gps.lat = positionGpsLla.c[0] * 1e7;
-			sensor_gps.lon = positionGpsLla.c[1] * 1e7;
-			sensor_gps.alt = positionGpsLla.c[2] * 1e3;
-			sensor_gps.alt_ellipsoid = sensor_gps.alt;
+			sensor_gps.latitude_deg = positionGpsLla.c[0];
+			sensor_gps.longitude_deg = positionGpsLla.c[1];
+			sensor_gps.altitude_msl_m = positionGpsLla.c[2];
+			sensor_gps.altitude_ellipsoid_m = sensor_gps.altitude_msl_m;
 
 			sensor_gps.vel_ned_valid = true;
 			sensor_gps.vel_n_m_s = velocityGpsNed.c[0];

src/drivers/ins/vectornav/VectorNav.cpp

@@ -386,9 +386,9 @@ struct msp_raw_gps_t {
 	uint8_t  numSat;
 	int32_t  lat;           // 1 / 10000000 deg
 	int32_t  lon;           // 1 / 10000000 deg
-	int16_t  alt;           // meters
+	int16_t  alt;           // centimeters since MSP API 1.39, meters before
 	int16_t  groundSpeed;   // cm/s
-	int16_t  groundCourse;  // unit: degree x 10
+	int16_t  groundCourse;  // unit: degree x 100, centidegrees
 	uint16_t hdop;
 } __attribute__((packed));
 

src/drivers/osd/msp_osd/msp_defines.h

@@ -314,9 +314,9 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position,
 	msp_raw_gps_t raw_gps {0};
 
 	if (vehicle_gps_position.fix_type >= 2) {
-		raw_gps.lat = vehicle_gps_position.lat;
-		raw_gps.lon = vehicle_gps_position.lon;
-		raw_gps.alt =  vehicle_gps_position.alt / 10;
+		raw_gps.lat = static_cast<int32_t>(vehicle_gps_position.latitude_deg * 1e7);
+		raw_gps.lon = static_cast<int32_t>(vehicle_gps_position.longitude_deg * 1e7);
+		raw_gps.alt = static_cast<int16_t>(vehicle_gps_position.altitude_msl_m * 100.0);
 
 		float course = math::degrees(vehicle_gps_position.cog_rad);
 
@@ -432,14 +432,14 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position,
 	msp_altitude_t altitude {0};
 
 	if (vehicle_gps_position.fix_type >= 2) {
-		altitude.estimatedActualPosition = vehicle_gps_position.alt / 10;
+		altitude.estimatedActualPosition = static_cast<int32_t>(vehicle_gps_position.altitude_msl_m * 100.0);	// cm
 
 	} else {
 		altitude.estimatedActualPosition = 0;
 	}
 
 	if (estimator_status.solution_status_flags & (1 << 5)) {
-		altitude.estimatedActualVelocity = -vehicle_local_position.vz * 10; //m/s to cm/s
+		altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s
 
 	} else {
 		altitude.estimatedActualVelocity = 0;

src/drivers/osd/msp_osd/uorb_to_msp.cpp

@@ -241,11 +241,11 @@ void CrsfRc::Run()
 				sensor_gps_s sensor_gps;
 
 				if (_vehicle_gps_position_sub.update(&sensor_gps)) {
-					int32_t latitude = sensor_gps.lat;
-					int32_t longitude = sensor_gps.lon;
+					int32_t latitude = static_cast<int32_t>(round(sensor_gps.latitude_deg * 1e7));
+					int32_t longitude = static_cast<int32_t>(round(sensor_gps.longitude_deg * 1e7));
 					uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f;
 					uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f;
-					uint16_t altitude = sensor_gps.alt + 1000;
+					uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m * 1e3) + 1000;
 					uint8_t num_satellites = sensor_gps.satellites_used;
 					this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites);
 				}

src/drivers/rc/crsf_rc/CrsfRc.cpp

@@ -96,11 +96,11 @@ bool CRSFTelemetry::send_gps()
 		return false;
 	}
 
-	int32_t latitude = vehicle_gps_position.lat;
-	int32_t longitude = vehicle_gps_position.lon;
+	int32_t latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));
+	int32_t longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));
 	uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
 	uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
-	uint16_t altitude = vehicle_gps_position.alt + 1000;
+	uint16_t altitude = static_cast<int16_t>(round(vehicle_gps_position.altitude_msl_m + 1.0));
 	uint8_t num_satellites = vehicle_gps_position.satellites_used;
 
 	return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed,

src/drivers/rc_input/crsf_telemetry.cpp

@@ -110,9 +110,9 @@ bool GHSTTelemetry::send_gps1_status()
 		return false;
 	}
 
-	int32_t latitude = vehicle_gps_position.lat;				// 1e-7 degrees
-	int32_t longitude = vehicle_gps_position.lon;				// 1e-7 degrees
-	uint16_t altitude = vehicle_gps_position.alt / 1000;			// mm -> m
+	int32_t latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));        // 1e-7 degrees
+	int32_t longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));      // 1e-7 degrees
+	uint16_t altitude = static_cast<int16_t>(round(vehicle_gps_position.altitude_msl_m));           // meters
 
 	return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude);
 }

src/drivers/rc_input/ghst_telemetry.cpp

@@ -268,9 +268,9 @@ void BST::RunImpl()
 		if (gps.fix_type >= 3 && gps.eph < 50.0f) {
 			BSTPacket<BSTGPSPosition> bst_gps = {};
 			bst_gps.type = 0x02;
-			bst_gps.payload.lat = swap_int32(gps.lat);
-			bst_gps.payload.lon = swap_int32(gps.lon);
-			bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000);
+			bst_gps.payload.lat = swap_int32(static_cast<int32_t>(round(gps.latitude_deg * 1e7)));
+			bst_gps.payload.lon = swap_int32(static_cast<int32_t>(round(gps.longitude_deg * 1e7)));
+			bst_gps.payload.alt = swap_int16(static_cast<int16_t>(round(gps.altitude_msl_m)) + 1000);
 			bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f);
 			bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F);
 			bst_gps.payload.sats = gps.satellites_used;