fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 10:23: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/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+2 -1
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));
}
}
}
src/modules/commander/HomePosition.cpp
+8 -8
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;
src/modules/commander/HomePosition.hpp
+2 -2
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};
};
src/modules/commander/baro_calibration.cpp
+1 -1
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;
src/modules/commander/mag_calibration.cpp
+4 -4
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;
}
}
src/modules/ekf2/EKF2.cpp
+4 -4
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));
}
}
src/modules/local_position_estimator/sensors/gps.cpp
+3 -3
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;
src/modules/logger/logger.cpp
+2 -1
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
src/modules/mavlink/mavlink_receiver.cpp
+4 -4
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;
src/modules/mavlink/streams/GPS2_RAW.hpp
+3 -3
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)
src/modules/mavlink/streams/GPS_RAW_INT.hpp
+4 -4
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
src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp
+3 -3
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
src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp
+3 -3
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]
src/modules/navigator/geofence.cpp
+2 -2
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);
}
}
}
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
+35 -35
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;
}
src/modules/sensors/vehicle_gps_position/gps_blending.hpp
+1 -1
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
src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp
+7 -7
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)
src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
+5 -5
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)
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+4 -4
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;