dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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-06-04 10:53:47 -05:00 committed by Beat Küng
parent f82785a322
commit f000238987
38 changed files with 169 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/SensorGps.msg
View File

@ -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)

6
src/drivers/cyphal/Publishers/udral/Gnss.hpp
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_];

2
src/drivers/gps/devices

@ -1 +1 @@
Subproject commit 6eabe40e58f94b997a2347aff34b7aa0bc093dd5
Subproject commit 261480cb78a3ba60cdda2dcc95b874ebc2c9312c

8
src/drivers/ins/vectornav/VectorNav.cpp
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];

4
src/drivers/osd/msp_osd/msp_defines.h
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));

10
src/drivers/osd/msp_osd/uorb_to_msp.cpp
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;

6
src/drivers/rc/crsf_rc/CrsfRc.cpp
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);
}

6
src/drivers/rc_input/crsf_telemetry.cpp
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,

6
src/drivers/rc_input/ghst_telemetry.cpp
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);
}

6
src/drivers/telemetry/bst/bst.cpp
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;

10
src/drivers/telemetry/frsky_telemetry/sPort_data.cpp
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);
}

6
src/drivers/telemetry/hott/messages.cpp
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;

18
src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp
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;

3
src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp
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;

8
src/drivers/uavcan/sensors/gnss.cpp
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

8
src/drivers/uavcannode/Publishers/GnssFix2.hpp
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;

16
src/examples/fake_gps/FakeGps.cpp
View File

@ -35,12 +35,12 @@
using namespace time_literals;
FakeGps::FakeGps(double latitude_deg, double longitude_deg, float altitude_m) :
FakeGps::FakeGps(double latitude_deg, double longitude_deg, double altitude_m) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
_latitude(latitude_deg * 10e6),
_longitude(longitude_deg * 10e6),
_altitude(altitude_m * 10e2f)
_latitude(latitude_deg),
_longitude(longitude_deg),
_altitude(altitude_m)
{
}
@ -60,10 +60,10 @@ void FakeGps::Run()
sensor_gps_s sensor_gps{};
sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954;
sensor_gps.lat = _latitude;
sensor_gps.lon = _longitude;
sensor_gps.alt = _altitude;
sensor_gps.alt_ellipsoid = _altitude;
sensor_gps.latitude_deg = _latitude;
sensor_gps.longitude_deg = _longitude;
sensor_gps.altitude_msl_m = _altitude;
sensor_gps.altitude_ellipsoid_m = _altitude;
sensor_gps.s_variance_m_s = 0.3740f;
sensor_gps.c_variance_rad = 0.6737f;
sensor_gps.eph = 2.1060f;

8
src/examples/fake_gps/FakeGps.hpp
View File

@ -45,7 +45,7 @@
class FakeGps : public ModuleBase<FakeGps>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, float altitude_m = 30.1f);
FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, double altitude_m = 30.1);
~FakeGps() override = default;
@ -67,7 +67,7 @@ private:
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
int32_t _latitude{296603018}; // Latitude in 1e-7 degrees
int32_t _longitude{-823160500}; // Longitude in 1e-7 degrees
int32_t _altitude{30100}; // Altitude in 1e-3 meters above MSL, (millimetres)
double _latitude{29.6603018}; // Latitude in degrees
double _longitude{-82.3160500}; // Longitude in degrees
double _altitude{30.1}; // Altitude in meters above MSL, (millimetres)
};

4
src/examples/fake_magnetometer/FakeMagnetometer.cpp
View File

@ -66,8 +66,8 @@ void FakeMagnetometer::Run()
if (_vehicle_gps_position_sub.copy(&gps)) {
if (gps.eph < 1000) {
const double lat = gps.lat / 1.e7;
const double lon = gps.lon / 1.e7;
const double lat = gps.latitude_deg;
const double lon = gps.longitude_deg;
// magnetic field data returned by the geo library using the current GPS position
const float mag_declination_gps = get_mag_declination_radians(lat, lon);

3
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -219,7 +219,8 @@ void AttitudeEstimatorQ::update_gps_position()
if (_vehicle_gps_position_sub.update(&gps)) {
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
// set magnetic declination automatically
update_mag_declination(get_mag_declination_radians(gps.lat, gps.lon));
update_mag_declination(get_mag_declination_radians((float)gps.latitude_deg,
(float)gps.longitude_deg));
}
}
}

16
src/modules/commander/HomePosition.cpp
View File

@ -152,10 +152,10 @@ void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, flo
void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos)
{
fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt);
fillGlobalHomePos(home, gpos.lat, gpos.lon, (double)gpos.alt);
}
void HomePosition::fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt)
void HomePosition::fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt)
{
home.lat = lat;
home.lon = lon;
@ -189,7 +189,7 @@ void HomePosition::setInAirHomePosition()
ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon);
const float home_alt = gpos.alt + home.z;
fillGlobalHomePos(home, home_lat, home_lon, home_alt);
fillGlobalHomePos(home, home_lat, home_lon, (double)home_alt);
setHomePosValid();
home.timestamp = hrt_absolute_time();
@ -206,8 +206,8 @@ void HomePosition::setInAirHomePosition()
double home_lon;
ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon);
const float home_alt = _gps_alt + home.z;
fillGlobalHomePos(home, home_lat, home_lon, home_alt);
const double home_alt = _gps_alt + (double)home.z;
fillGlobalHomePos(home, home_lat, home_lon, (double)home_alt);
setHomePosValid();
home.timestamp = hrt_absolute_time();
@ -304,9 +304,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
sensor_gps_s vehicle_gps_position;
_vehicle_gps_position_sub.copy(&vehicle_gps_position);
_gps_lat = static_cast<double>(vehicle_gps_position.lat) * 1e-7;
_gps_lon = static_cast<double>(vehicle_gps_position.lon) * 1e-7;
_gps_alt = static_cast<float>(vehicle_gps_position.alt) * 1e-3f;
_gps_lat = vehicle_gps_position.latitude_deg;
_gps_lon = vehicle_gps_position.longitude_deg;
_gps_alt = vehicle_gps_position.altitude_msl_m;
_gps_eph = vehicle_gps_position.eph;
_gps_epv = vehicle_gps_position.epv;

4
src/modules/commander/HomePosition.hpp
View File

@ -68,7 +68,7 @@ private:
static void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos);
static void fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading);
static void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos);
static void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt);
static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt);
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
@ -83,7 +83,7 @@ private:
bool _gps_position_for_home_valid{false};
double _gps_lat{0};
double _gps_lon{0};
float _gps_alt{0.f};
double _gps_alt{0};
float _gps_eph{0.f};
float _gps_epv{0.f};
};

2
src/modules/commander/baro_calibration.cpp
View File

@ -136,7 +136,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub)
if ((hrt_elapsed_time(&sensor_gps.timestamp) < 1_s)
&& (sensor_gps.fix_type >= 2) && (sensor_gps.epv < 100)) {
float alt = sensor_gps.alt * 0.001f;
float alt = (float)sensor_gps.altitude_msl_m;
if (PX4_ISFINITE(gps_altitude_sum)) {
gps_altitude_sum += alt;

8
src/modules/commander/mag_calibration.cpp
View File

@ -237,8 +237,8 @@ static float get_sphere_radius()
if (gps_sub.copy(&gps)) {
if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) {
const double lat = gps.lat / 1.e7;
const double lon = gps.lon / 1.e7;
const double lat = gps.latitude_deg;
const double lon = gps.longitude_deg;
// magnetic field data returned by the geo library using the current GPS position
return get_mag_strength_gauss(lat, lon);
@ -960,8 +960,8 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
if (vehicle_gps_position_sub.copy(&gps)) {
if ((gps.timestamp != 0) && (gps.eph < 1000)) {
latitude = gps.lat / 1.e7f;
longitude = gps.lon / 1.e7f;
latitude = (float)gps.latitude_deg;
longitude = (float)gps.longitude_deg;
mag_earth_available = true;
}
}

8
src/modules/ekf2/EKF2.cpp
View File

@ -2245,9 +2245,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
gpsMessage gps_msg{
.time_usec = vehicle_gps_position.timestamp,
.lat = vehicle_gps_position.lat,
.lon = vehicle_gps_position.lon,
.alt = vehicle_gps_position.alt,
.lat = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7)),
.lon = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7)),
.alt = static_cast<int32_t>(round(vehicle_gps_position.altitude_msl_m * 1e3)),
.yaw = vehicle_gps_position.heading,
.yaw_offset = vehicle_gps_position.heading_offset,
.yaw_accuracy = vehicle_gps_position.heading_accuracy,
@ -2269,7 +2269,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
_ekf.setGpsData(gps_msg);
_gps_time_usec = gps_msg.time_usec;
_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
_gps_alttitude_ellipsoid = static_cast<int32_t>(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3));
}
}

6
src/modules/local_position_estimator/sensors/gps.cpp
View File

@ -92,9 +92,9 @@ int BlockLocalPositionEstimator::gpsMeasure(Vector<double, n_y_gps> &y)
{
// gps measurement
y.setZero();
y(0) = _sub_gps.get().lat * 1e-7;
y(1) = _sub_gps.get().lon * 1e-7;
y(2) = _sub_gps.get().alt * 1e-3;
y(0) = _sub_gps.get().latitude_deg;
y(1) = _sub_gps.get().longitude_deg;
y(2) = _sub_gps.get().altitude_msl_m;
y(3) = (double)_sub_gps.get().vel_n_m_s;
y(4) = (double)_sub_gps.get().vel_e_m_s;
y(5) = (double)_sub_gps.get().vel_d_m_s;

3
src/modules/logger/logger.cpp
View File

@ -2103,7 +2103,8 @@ void Logger::write_version(LogType type)
// data versioning: increase this on every larger data change (format/semantic)
// 1: switch to FIFO drivers (disabled on-chip DLPF)
write_info(type, "ver_data_format", static_cast<uint32_t>(1));
// 2: changed lat/lon/alt* to double to accommodate RTK GPS centimeter level precision
write_info(type, "ver_data_format", static_cast<uint32_t>(2));
#ifndef BOARD_HAS_NO_UUID

8
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -2351,10 +2351,10 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
gps.device_id = device_id.devid;
gps.lat = hil_gps.lat;
gps.lon = hil_gps.lon;
gps.alt = hil_gps.alt;
gps.alt_ellipsoid = hil_gps.alt;
gps.latitude_deg = hil_gps.lat * 1e-7;
gps.longitude_deg = hil_gps.lon * 1e-7;
gps.altitude_msl_m = hil_gps.alt * 1e-3;
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
gps.s_variance_m_s = 0.25f;
gps.c_variance_rad = 0.5f;

6
src/modules/mavlink/streams/GPS2_RAW.hpp
View File

@ -71,9 +71,9 @@ private:
msg.time_usec = gps.timestamp;
msg.fix_type = gps.fix_type;
msg.lat = gps.lat;
msg.lon = gps.lon;
msg.alt = gps.alt;
msg.lat = static_cast<int32_t>(round(gps.latitude_deg * 1e7));
msg.lon = static_cast<int32_t>(round(gps.longitude_deg * 1e7));
msg.alt = static_cast<int32_t>(round(gps.altitude_msl_m * 1e3)); // convert [m] to [mm]
msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless)
msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless)

8
src/modules/mavlink/streams/GPS_RAW_INT.hpp
View File

@ -70,9 +70,9 @@ private:
if (_sensor_gps_sub.update(&gps)) {
msg.time_usec = gps.timestamp;
msg.fix_type = gps.fix_type;
msg.lat = gps.lat;
msg.lon = gps.lon;
msg.alt = gps.alt;
msg.lat = static_cast<int32_t>(round(gps.latitude_deg * 1e7));
msg.lon = static_cast<int32_t>(round(gps.longitude_deg * 1e7));
msg.alt = static_cast<int32_t>(round(gps.altitude_msl_m * 1e3)); // convert [m] to [mm]
msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless)
msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless)
@ -85,7 +85,7 @@ private:
msg.cog = math::degrees(matrix::wrap_2pi(gps.cog_rad)) * 1e2f;
msg.satellites_visible = gps.satellites_used;
msg.alt_ellipsoid = gps.alt_ellipsoid;
msg.alt_ellipsoid = static_cast<int32_t>(round(gps.altitude_ellipsoid_m * 1e3)); // convert [m] to [mm]
msg.h_acc = gps.eph * 1e3f; // position uncertainty in mm
msg.v_acc = gps.epv * 1e3f; // altitude uncertainty in mm
msg.vel_acc = gps.s_variance_m_s * 1e3f; // speed uncertainty in mm

6
src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp
View File

@ -165,12 +165,12 @@ private:
}
if (vehicle_gps_position.fix_type >= 2) {
msg.latitude = vehicle_gps_position.lat;
msg.longitude = vehicle_gps_position.lon;
msg.latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));
msg.longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));
// altitude_geodetic
if (vehicle_gps_position.fix_type >= 3) {
msg.altitude_geodetic = vehicle_gps_position.alt * 1e-3f;
msg.altitude_geodetic = static_cast<float>(round(vehicle_gps_position.altitude_msl_m)); // [m]
}
// horizontal_accuracy

6
src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp
View File

@ -86,9 +86,9 @@ private:
// Required update for dynamic message is 5 [Hz]
mavlink_uavionix_adsb_out_dynamic_t dynamic_msg = {
.utcTime = static_cast<uint32_t>(vehicle_gps_position.time_utc_usec / 1000000ULL),
.gpsLat = vehicle_gps_position.lat,
.gpsLon = vehicle_gps_position.lon,
.gpsAlt = vehicle_gps_position.alt_ellipsoid,
.gpsLat = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7)),
.gpsLon = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7)),
.gpsAlt = static_cast<int32_t>(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3)), // convert [m] to [mm]
.baroAltMSL = static_cast<int32_t>(vehicle_air_data.baro_pressure_pa / 100.0f), // convert [Pa] to [mBar]
.accuracyHor = static_cast<uint32_t>(vehicle_gps_position.eph * 1000.0f), // convert [m] to [mm]
.accuracyVert = static_cast<uint16_t>(vehicle_gps_position.epv * 100.0f), // convert [m] to [cm]

4
src/modules/navigator/geofence.cpp
View File

@ -220,7 +220,7 @@ bool Geofence::check(const vehicle_global_position_s &global_position, const sen
return checkAll(global_position);
} else {
return checkAll(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3);
return checkAll(gps_position.latitude_deg, gps_position.longitude_deg, gps_position.altitude_msl_m);
}
} else {
@ -232,7 +232,7 @@ bool Geofence::check(const vehicle_global_position_s &global_position, const sen
return checkAll(global_position, baro_altitude_amsl);
} else {
return checkAll(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, baro_altitude_amsl);
return checkAll(gps_position.latitude_deg, gps_position.longitude_deg, baro_altitude_amsl);
}
}
}

70
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
View File

@ -70,7 +70,7 @@ void GpsBlending::update(uint64_t hrt_now_us)
// Check for new data on selected GPS, and clear blend offsets
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
_NE_pos_offset_m[i].zero();
_hgt_offset_mm[i] = 0.0f;
_hgt_offset_m[i] = 0.0;
}
// Only use a secondary instance if the fallback is allowed
@ -443,38 +443,38 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
// Convert each GPS position to a local NEU offset relative to the reference position
Vector2f blended_NE_offset_m{0, 0};
float blended_alt_offset_mm = 0.0f;
double blended_alt_offset_m = 0.0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
if ((blend_weights[i] > 0.0f) && (i != gps_best_index)) {
// calculate the horizontal offset
Vector2f horiz_offset{};
get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7),
(_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
get_vector_to_next_waypoint(gps_blended_state.latitude_deg, gps_blended_state.longitude_deg,
_gps_state[i].latitude_deg, _gps_state[i].longitude_deg,
&horiz_offset(0), &horiz_offset(1));
// sum weighted offsets
blended_NE_offset_m += horiz_offset * blend_weights[i];
// calculate vertical offset
float vert_offset = (float)(_gps_state[i].alt - gps_blended_state.alt);
// calculate vertical offset, meters
double vert_offset_m = _gps_state[i].altitude_msl_m - gps_blended_state.altitude_msl_m;
// sum weighted offsets
blended_alt_offset_mm += vert_offset * blend_weights[i];
blended_alt_offset_m += vert_offset_m * (double)blend_weights[i];
}
}
// Add the sum of weighted offsets to the reference position to obtain the blended position
const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7;
const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7;
const double lat_deg_now = gps_blended_state.latitude_deg;
const double lon_deg_now = gps_blended_state.longitude_deg;
double lat_deg_res = 0;
double lon_deg_res = 0;
add_vector_to_global_position(lat_deg_now, lon_deg_now,
blended_NE_offset_m(0), blended_NE_offset_m(1),
&lat_deg_res, &lon_deg_res);
gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res);
gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res);
gps_blended_state.alt += (int32_t)blended_alt_offset_mm;
gps_blended_state.latitude_deg = lat_deg_res;
gps_blended_state.longitude_deg = lon_deg_res;
gps_blended_state.altitude_msl_m += blended_alt_offset_m;
// Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
int8_t gps_best_yaw_index = -1;
@ -516,30 +516,30 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
// Calculate a filtered position delta for each GPS relative to the blended solution state
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
Vector2f offset;
get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
(gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7),
get_vector_to_next_waypoint(_gps_state[i].latitude_deg, _gps_state[i].longitude_deg,
gps_blended_state.latitude_deg, gps_blended_state.longitude_deg,
&offset(0), &offset(1));
_NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]);
_hgt_offset_mm[i] = (float)(gps_blended_state.alt - _gps_state[i].alt) * alpha[i] +
_hgt_offset_mm[i] * (1.0f - alpha[i]);
_hgt_offset_m[i] = (gps_blended_state.altitude_msl_m - _gps_state[i].altitude_msl_m) * (double)alpha[i] +
_hgt_offset_m[i] * (1.0 - (double)alpha[i]);
}
// calculate offset limits from the largest difference between receivers
Vector2f max_ne_offset{};
float max_alt_offset = 0;
double max_alt_offset = 0.0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
for (uint8_t j = i; j < GPS_MAX_RECEIVERS_BLEND; j++) {
if (i != j) {
Vector2f offset;
get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7),
(_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7),
get_vector_to_next_waypoint(_gps_state[i].latitude_deg, _gps_state[i].longitude_deg,
_gps_state[j].latitude_deg, _gps_state[j].longitude_deg,
&offset(0), &offset(1));
max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0)));
max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1)));
max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt)));
max_ne_offset(0) = fmax(max_ne_offset(0), fabsf(offset(0)));
max_ne_offset(1) = fmax(max_ne_offset(1), fabsf(offset(1)));
max_alt_offset = fmax(max_alt_offset, fabs(_gps_state[i].altitude_msl_m - _gps_state[j].altitude_msl_m));
}
}
}
@ -548,7 +548,7 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
_NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0));
_NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1));
_hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset);
_hgt_offset_m[i] = constrain(_hgt_offset_m[i], -max_alt_offset, max_alt_offset);
}
}
@ -558,26 +558,26 @@ void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state,
// Convert each GPS position to a local NEU offset relative to the reference position
// which is defined as the positon of the blended solution calculated from non offset corrected data
Vector2f blended_NE_offset_m{0, 0};
float blended_alt_offset_mm = 0.0f;
double blended_alt_offset_m = 0.0;
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
if (blend_weights[i] > 0.0f) {
// Add the sum of weighted offsets to the reference position to obtain the blended position
const double lat_deg_orig = (double)_gps_state[i].lat * 1.0e-7;
const double lon_deg_orig = (double)_gps_state[i].lon * 1.0e-7;
const double lat_deg_orig = _gps_state[i].latitude_deg;
const double lon_deg_orig = _gps_state[i].longitude_deg;
double lat_deg_offset_res = 0;
double lon_deg_offset_res = 0;
add_vector_to_global_position(lat_deg_orig, lon_deg_orig,
_NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1),
&lat_deg_offset_res, &lon_deg_offset_res);
float alt_offset = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i];
double alt_offset_m = _gps_state[i].altitude_msl_m + _hgt_offset_m[i];
// calculate the horizontal offset
Vector2f horiz_offset{};
get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7),
get_vector_to_next_waypoint(gps_blended_state.latitude_deg, gps_blended_state.longitude_deg,
lat_deg_offset_res, lon_deg_offset_res,
&horiz_offset(0), &horiz_offset(1));
@ -585,23 +585,23 @@ void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state,
blended_NE_offset_m += horiz_offset * blend_weights[i];
// calculate vertical offset
float vert_offset = alt_offset - gps_blended_state.alt;
double vert_offset_m = alt_offset_m - gps_blended_state.altitude_msl_m;
// sum weighted offsets
blended_alt_offset_mm += vert_offset * blend_weights[i];
blended_alt_offset_m += vert_offset_m * (double)blend_weights[i];
}
}
// Add the sum of weighted offsets to the reference position to obtain the blended position
const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7;
const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7;
const double lat_deg_now = gps_blended_state.latitude_deg;
const double lon_deg_now = gps_blended_state.longitude_deg;
double lat_deg_res = 0;
double lon_deg_res = 0;
add_vector_to_global_position(lat_deg_now, lon_deg_now,
blended_NE_offset_m(0), blended_NE_offset_m(1),
&lat_deg_res, &lon_deg_res);
gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res);
gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res);
gps_blended_state.alt = gps_blended_state.alt + (int32_t)blended_alt_offset_mm;
gps_blended_state.latitude_deg = lat_deg_res;
gps_blended_state.longitude_deg = lon_deg_res;
gps_blended_state.altitude_msl_m = gps_blended_state.altitude_msl_m + blended_alt_offset_m;
}

2
src/modules/sensors/vehicle_gps_position/gps_blending.hpp
View File

@ -131,7 +131,7 @@ private:
bool _is_new_output_data_available{false};
matrix::Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
float _hgt_offset_mm[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
double _hgt_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (meters)
uint64_t _time_prev_us[GPS_MAX_RECEIVERS_BLEND] {}; ///< the previous value of time_us for that GPS instance - used to detect new data.
uint8_t _gps_time_ref_index{0}; ///< index of the receiver that is used as the timing reference for the blending update

14
src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp
View File

@ -60,10 +60,10 @@ sensor_gps_s GpsBlendingTest::getDefaultGpsData()
sensor_gps_s gps_data{};
gps_data.timestamp = _time_now_us - 10e3;
gps_data.time_utc_usec = 0;
gps_data.lat = 47e7;
gps_data.lon = 9e7;
gps_data.alt = 800e3;
gps_data.alt_ellipsoid = 800e3;
gps_data.latitude_deg = 47.0;
gps_data.longitude_deg = 9.0;
gps_data.altitude_msl_m = 800.0;
gps_data.altitude_ellipsoid_m = 800.0;
gps_data.s_variance_m_s = 0.2f;
gps_data.c_variance_rad = 0.5f;
gps_data.eph = 0.7f;
@ -213,9 +213,9 @@ TEST_F(GpsBlendingTest, dualReceiverBlendingHPos)
EXPECT_FLOAT_EQ(gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than
EXPECT_EQ(gps_blending.getOutputGpsData().timestamp, gps_data0.timestamp);
EXPECT_EQ(gps_blending.getOutputGpsData().timestamp_sample, gps_data0.timestamp_sample);
EXPECT_EQ(gps_blending.getOutputGpsData().lat, gps_data0.lat);
EXPECT_EQ(gps_blending.getOutputGpsData().lon, gps_data0.lon);
EXPECT_EQ(gps_blending.getOutputGpsData().alt, gps_data0.alt);
EXPECT_EQ(gps_blending.getOutputGpsData().latitude_deg, gps_data0.latitude_deg);
EXPECT_EQ(gps_blending.getOutputGpsData().latitude_deg, gps_data0.latitude_deg);
EXPECT_EQ(gps_blending.getOutputGpsData().altitude_msl_m, gps_data0.altitude_msl_m);
}
TEST_F(GpsBlendingTest, dualReceiverFailover)

10
src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
View File

@ -116,7 +116,7 @@ void SensorGpsSim::Run()
double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
float altitude = gpos.alt + (generate_wgn() * 0.5f);
double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f));
Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f);
@ -153,10 +153,10 @@ void SensorGpsSim::Run()
sensor_gps.timestamp_sample = gpos.timestamp_sample;
sensor_gps.time_utc_usec = 0;
sensor_gps.device_id = device_id.devid;
sensor_gps.lat = roundf(latitude * 1e7); // Latitude in 1E-7 degrees
sensor_gps.lon = roundf(longitude * 1e7); // Longitude in 1E-7 degrees
sensor_gps.alt = roundf(altitude * 1000.f); // Altitude in 1E-3 meters above MSL, (millimetres)
sensor_gps.alt_ellipsoid = sensor_gps.alt;
sensor_gps.latitude_deg = latitude; // Latitude in degrees
sensor_gps.longitude_deg = longitude; // Longitude in degrees
sensor_gps.altitude_msl_m = altitude; // Altitude in meters above MSL
sensor_gps.altitude_ellipsoid_m = altitude;
sensor_gps.noise_per_ms = 0;
sensor_gps.jamming_indicator = 0;
sensor_gps.vel_m_s = sqrtf(gps_vel(0) * gps_vel(0) + gps_vel(1) * gps_vel(1)); // GPS ground speed, (metres/sec)

8
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
View File

@ -402,10 +402,10 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
if (!_gps_blocked) {
sensor_gps_s gps{};
gps.lat = hil_gps.lat;
gps.lon = hil_gps.lon;
gps.alt = hil_gps.alt;
gps.alt_ellipsoid = hil_gps.alt;
gps.latitude_deg = hil_gps.lat / 1e7;
gps.longitude_deg = hil_gps.lon / 1e7;
gps.altitude_msl_m = hil_gps.alt / 1e3;
gps.altitude_ellipsoid_m = hil_gps.alt / 1e3;
gps.s_variance_m_s = 0.25f;
gps.c_variance_rad = 0.5f;