fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 23:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
To match https://github.com/PX4/PX4-GPSDrivers/pull/132 - adding high precision RTK lat/lon/alt components
This commit is contained in:
Sergei Grichine
2023-07-13 07:50:09 +02:00
committed by Beat Küng
parent f82785a322
commit f000238987
38 changed files with 169 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/cyphal/Publishers/udral/Gnss.hpp
+3 -3
View File
@@ -66,9 +66,9 @@ public:
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
+1 -1
Submodule src/drivers/gps/devices updated: 6eabe40e58...261480cb78
src/drivers/ins/vectornav/VectorNav.cpp
+4 -4
View File
@@ -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/osd/msp_osd/msp_defines.h
+2 -2
View File
@@ -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/uorb_to_msp.cpp
+5 -5
View File
@@ -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/rc/crsf_rc/CrsfRc.cpp
+3 -3
View File
@@ -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_input/crsf_telemetry.cpp
+3 -3
View File
@@ -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/ghst_telemetry.cpp
+3 -3
View File
@@ -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/telemetry/bst/bst.cpp
+3 -3
View File
@@ -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;
src/drivers/telemetry/frsky_telemetry/sPort_data.cpp
+5 -5
View File
@@ -233,11 +233,11 @@ void sPort_send_GPS_LON(int uart)
/* send longitude */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
/* precision is approximately 0.1m */
uint32_t iLon = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lon);
uint32_t iLon = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg * 1e7);
iLon |= (1 << 31);
if (s_port_subscription_data->vehicle_gps_position_sub.get().lon < 0) { iLon |= (1 << 30); }
if (s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg < 0) { iLon |= (1 << 30); }
sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon);
}
@@ -246,9 +246,9 @@ void sPort_send_GPS_LAT(int uart)
{
/* send latitude */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */
uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lat);
uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg * 1e7);
if (s_port_subscription_data->vehicle_gps_position_sub.get().lat < 0) { iLat |= (1 << 30); }
if (s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg < 0) { iLat |= (1 << 30); }
sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat);
}
@@ -256,7 +256,7 @@ void sPort_send_GPS_LAT(int uart)
void sPort_send_GPS_ALT(int uart)
{
/* send altitude */
uint32_t iAlt = s_port_subscription_data->vehicle_gps_position_sub.get().alt / 10;
uint32_t iAlt = static_cast<uint32_t>(s_port_subscription_data->vehicle_gps_position_sub.get().altitude_msl_m * 1e2);
sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt);
}
src/drivers/telemetry/hott/messages.cpp
+3 -3
View File
@@ -242,7 +242,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
/* Get latitude in degrees, minutes and seconds */
double lat = ((double)(gps.lat)) * 1e-7d;
double lat = gps.latitude_deg;
/* Set the N or S specifier */
msg.latitude_ns = 0;
@@ -265,7 +265,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
/* Get longitude in degrees, minutes and seconds */
double lon = ((double)(gps.lon)) * 1e-7d;
double lon = gps.longitude_deg;
/* Set the E or W specifier */
msg.longitude_ew = 0;
@@ -285,7 +285,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
/* Altitude */
uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f);
uint16_t alt = (uint16_t)(round(gps.altitude_msl_m) + 500.0);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp
+10 -8
View File
@@ -455,7 +455,7 @@ void SagetechMXS::determine_furthest_aircraft()
continue;
}
const float distance = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE,
const float distance = get_distance_to_next_waypoint(_gps.latitude_deg, _gps.longitude_deg,
vehicle_list[index].lat,
vehicle_list[index].lon);
@@ -492,8 +492,8 @@ void SagetechMXS::handle_vehicle(const transponder_report_s &vehicle)
// needs to handle updating the vehicle list, keeping track of which vehicles to drop
// and which to keep, allocating new vehicles, and publishing to the transponder_report topic
uint16_t index = list_size_allocated + 1; // Make invalid to start with.
const bool my_loc_is_zero = (_gps.lat == 0) && (_gps.lon == 0);
const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE,
const bool my_loc_is_zero = (fabs(_gps.latitude_deg) < DBL_EPSILON) && (fabs(_gps.longitude_deg) < DBL_EPSILON);
const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.latitude_deg, _gps.longitude_deg,
vehicle.lat, vehicle.lon);
const bool is_tracked_in_list = find_index(vehicle, &index);
// const bool is_special = is_special_vehicle(vehicle.icao_address);
@@ -745,7 +745,8 @@ void SagetechMXS::send_operating_msg()
mxs_state.op.altRes25 =
!mxs_state.inst.altRes100; // Host Altitude Resolution from install
mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet
mxs_state.op.altitude = static_cast<int32_t>(_gps.altitude_msl_m *
SAGETECH_SCALE_M_TO_FT); // Height above sealevel in feet
mxs_state.op.identOn = _adsb_ident.get();
@@ -806,8 +807,8 @@ void SagetechMXS::send_gps_msg()
}
// Get Vehicle Longitude and Latitude and Convert to string
const int32_t longitude = _gps.lon;
const int32_t latitude = _gps.lat;
const int32_t longitude = static_cast<int32_t>(_gps.longitude_deg * 1e7);
const int32_t latitude = static_cast<int32_t>(_gps.latitude_deg * 1e7);
const double lon_deg = longitude * 1.0E-7 * (longitude < 0 ? -1 : 1);
const double lon_minutes = (lon_deg - int(lon_deg)) * 60;
snprintf((char *)&gps.longitude, 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes,
@@ -836,7 +837,7 @@ void SagetechMXS::send_gps_msg()
snprintf((char *)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min,
tm->tm_sec + (_gps.time_utc_usec % 1000000) * 1.0e-6);
gps.height = _gps.alt_ellipsoid * 1E-3;
gps.height = (float)_gps.altitude_ellipsoid_m;
// checkGPSInputs(&gps);
last.msg.type = SG_MSG_TYPE_HOST_GPS;
@@ -1284,7 +1285,8 @@ void SagetechMXS::auto_config_operating()
mxs_state.op.altHostAvlbl = false;
mxs_state.op.altRes25 = true; // Host Altitude Resolution from install
mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet
mxs_state.op.altitude = static_cast<int32_t>(_gps.altitude_msl_m *
SAGETECH_SCALE_M_TO_FT); // Height above sealevel in feet
mxs_state.op.identOn = false;
src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp
+1 -2
View File
@@ -142,7 +142,7 @@ private:
static constexpr float SAGETECH_SCALE_KNOTS_TO_M_PER_SEC{0.514444F};
static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_KNOTS{1.94384F};
static constexpr float SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC{0.00508F};
static constexpr float SAGETECH_SCALE_MM_TO_FT{0.00328084F};
static constexpr double SAGETECH_SCALE_M_TO_FT{3.28084};
static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN{196.85F};
static constexpr uint8_t ADSB_ALTITUDE_TYPE_PRESSURE_QNH{0};
static constexpr uint8_t ADSB_ALTITUDE_TYPE_GEOMETRIC{1};
@@ -156,7 +156,6 @@ private:
static constexpr uint16_t INVALID_SQUAWK{7777};
static constexpr unsigned BAUD_460800{0010004}; // B460800 not defined in MacOS termios
static constexpr unsigned BAUD_921600{0010007}; // B921600 not defined in MacOS termios
static constexpr double GPS_SCALE{1.0E-7};
// Stored variables
uint64_t _loop_count;
src/drivers/uavcan/sensors/gnss.cpp
+4 -4
View File
@@ -337,10 +337,10 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
*/
report.timestamp = hrt_absolute_time();
report.lat = msg.latitude_deg_1e8 / 10;
report.lon = msg.longitude_deg_1e8 / 10;
report.alt = msg.height_msl_mm;
report.alt_ellipsoid = msg.height_ellipsoid_mm;
report.latitude_deg = msg.latitude_deg_1e8 / 1e8;
report.longitude_deg = msg.longitude_deg_1e8 / 1e8;
report.altitude_msl_m = msg.height_msl_mm / 1e3;
report.altitude_ellipsoid_m = msg.height_ellipsoid_mm / 1e3;
if (valid_pos_cov) {
// Horizontal position uncertainty
src/drivers/uavcannode/Publishers/GnssFix2.hpp
+4 -4
View File
@@ -81,10 +81,10 @@ public:
fix2.gnss_time_standard = fix2.GNSS_TIME_STANDARD_UTC;
fix2.gnss_timestamp.usec = gps.time_utc_usec;
fix2.latitude_deg_1e8 = (int64_t)gps.lat * 10;
fix2.longitude_deg_1e8 = (int64_t)gps.lon * 10;
fix2.height_msl_mm = gps.alt;
fix2.height_ellipsoid_mm = gps.alt_ellipsoid;
fix2.latitude_deg_1e8 = (int64_t)(gps.latitude_deg * 1e8);
fix2.longitude_deg_1e8 = (int64_t)(gps.longitude_deg * 1e8);
fix2.height_msl_mm = (int32_t)(gps.altitude_msl_m * 1e3);
fix2.height_ellipsoid_mm = (int32_t)(gps.altitude_ellipsoid_m * 1e3);
fix2.status = gps.fix_type;
fix2.ned_velocity[0] = gps.vel_n_m_s;
fix2.ned_velocity[1] = gps.vel_e_m_s;