fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 16:03:33 +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/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;