fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 16:13:35 +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/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)