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

geo: Moved the map_projection_* functions and state into a self-contained C++ class

Browse Source
This commit is contained in:
Thomas Debrunner
2021-12-06 14:30:14 +01:00
committed by Matthias Grob
parent fe23718e2c
commit 8db7a6225b
38 changed files with 324 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
+16 -20
View File
@@ -700,8 +700,8 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
{
origin_time = _last_gps_origin_time_us;
latitude = math::degrees(_pos_ref.lat_rad);
longitude = math::degrees(_pos_ref.lon_rad);
latitude = _pos_ref.getProjectionReferenceLat();
longitude = _pos_ref.getProjectionReferenceLon();
origin_alt = _gps_alt_ref;
return _NED_origin_initialised;
}
@@ -714,33 +714,29 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
float current_alt = 0.f;
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (map_projection_initialized(&_pos_ref) && isHorizontalAidingActive()) {
map_projection_reproject(&_pos_ref, _state.pos(0), _state.pos(1), &current_lat, &current_lon);
if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
current_alt = -_state.pos(2) + _gps_alt_ref;
current_pos_available = true;
}
// reinitialize map projection to latitude, longitude, altitude, and reset position
if (map_projection_init_timestamped(&_pos_ref, latitude, longitude, _time_last_imu) == 0) {
if (current_pos_available) {
// reset horizontal position
Vector2f position;
map_projection_project(&_pos_ref, current_lat, current_lon, &position(0), &position(1));
resetHorizontalPositionTo(position);
_pos_ref.initReference(latitude, longitude, _time_last_imu);
if (current_pos_available) {
// reset horizontal position
Vector2f position = _pos_ref.project(current_lat, current_lon);
resetHorizontalPositionTo(position);
// reset altitude
_gps_alt_ref = altitude;
resetVerticalPositionTo(_gps_alt_ref - current_alt);
// reset altitude
_gps_alt_ref = altitude;
resetVerticalPositionTo(_gps_alt_ref - current_alt);
} else {
// reset altitude
_gps_alt_ref = altitude;
}
return true;
} else {
// reset altitude
_gps_alt_ref = altitude;
}
return false;
return true;
}
/*
src/modules/ekf2/EKF/estimator_interface.cpp
+1 -1
View File
@@ -197,7 +197,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
if (collect_gps(gps)) {
float lpos_x = 0.0f;
float lpos_y = 0.0f;
map_projection_project(&_pos_ref, (gps.lat / 1.0e7), (gps.lon / 1.0e7), &lpos_x, &lpos_y);
_pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7), lpos_x, lpos_y);
gps_sample_new.pos(0) = lpos_x;
gps_sample_new.pos(1) = lpos_y;
src/modules/ekf2/EKF/estimator_interface.h
+3 -3
View File
@@ -253,7 +253,7 @@ public:
const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
const bool &global_origin_valid() const { return _NED_origin_initialised; }
const map_projection_reference_s &global_origin() const { return _pos_ref; }
const MapProjection &global_origin() const { return _pos_ref; }
void print_status();
@@ -324,8 +324,8 @@ protected:
bool _gps_speed_valid{false};
float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin
float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin
struct map_projection_reference_s _pos_ref {}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
struct map_projection_reference_s _gps_pos_prev {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
MapProjection _pos_ref {}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
MapProjection _gps_pos_prev {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
src/modules/ekf2/EKF/gps_checks.cpp
+11 -10
View File
@@ -65,22 +65,23 @@ bool Ekf::collect_gps(const gps_message &gps)
const double lat = gps.lat * 1.0e-7;
const double lon = gps.lon * 1.0e-7;
if (!map_projection_initialized(&_pos_ref)) {
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
if (!_pos_ref.isInitialized()) {
_pos_ref.initReference(lat, lon, _time_last_imu);
// if we are already doing aiding, correct for the change in position since the EKF started navigating
if (isHorizontalAidingActive()) {
double est_lat;
double est_lon;
map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
_pos_ref.initReference(est_lat, est_lon, _time_last_imu);
}
}
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
_NED_origin_initialised = true;
_earth_rate_NED = calcEarthRateNED((float)_pos_ref.lat_rad);
_earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat()));
_last_gps_origin_time_us = _time_last_imu;
const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
@@ -169,7 +170,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
constexpr float filt_time_const = 10.0f;
const float dt = math::constrain(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.timestamp)) * 1e-6f, 0.001f, filt_time_const);
const float dt = math::constrain(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const);
const float filter_coef = dt / filt_time_const;
// The following checks are only valid when the vehicle is at rest
@@ -182,12 +183,12 @@ bool Ekf::gps_is_good(const gps_message &gps)
float delta_pos_e = 0.0f;
// calculate position movement since last GPS fix
if (_gps_pos_prev.timestamp > 0) {
map_projection_project(&_gps_pos_prev, lat, lon, &delta_pos_n, &delta_pos_e);
if (_gps_pos_prev.getProjectionReferenceTimestamp() > 0) {
_gps_pos_prev.project(lat, lon, delta_pos_n, delta_pos_e);
} else {
// no previous position has been set
map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
_gps_pos_prev.initReference(lat, lon, _time_last_imu);
_gps_alt_prev = 1e-3f * (float)gps.alt;
}
@@ -235,7 +236,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
}
// save GPS fix for next time
map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
_gps_pos_prev.initReference(lat, lon, _time_last_imu);
_gps_alt_prev = 1e-3f * (float)gps.alt;
// Check the filtered difference between GPS and EKF vertical velocity
src/modules/ekf2/EKF2.cpp
+4 -4
View File
@@ -716,7 +716,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
global_pos.timestamp_sample = timestamp;
// Position of local NED origin in GPS / WGS84 frame
map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon);
_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
float delta_xy[2];
_ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter);
@@ -867,9 +867,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
// Position of local NED origin in GPS / WGS84 frame
if (_ekf.global_origin_valid()) {
lpos.ref_timestamp = _ekf.global_origin().timestamp;
lpos.ref_lat = math::degrees(_ekf.global_origin().lat_rad); // Reference point latitude in degrees
lpos.ref_lon = math::degrees(_ekf.global_origin().lon_rad); // Reference point longitude in degrees
lpos.ref_timestamp = _ekf.global_origin().getProjectionReferenceTimestamp();
lpos.ref_lat = _ekf.global_origin().getProjectionReferenceLat(); // Reference point latitude in degrees
lpos.ref_lon = _ekf.global_origin().getProjectionReferenceLon(); // Reference point longitude in degrees
lpos.ref_alt = _ekf.getEkfGlobalOriginAltitude(); // Reference point in MSL altitude meters
lpos.xy_global = true;
lpos.z_global = true;
src/modules/ekf2/test/sensor_simulator/gps.cpp
+2 -2
View File
@@ -98,11 +98,11 @@ void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
double lat_new {0.0};
double lon_new {0.0};
map_projection_project(&_ekf->global_origin(), _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr);
_ekf->global_origin().project(_gps_data.lat * 1e-7, _gps_data.lon * 1e-7, hposN_curr, hposE_curr);
Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
map_projection_reproject(&_ekf->global_origin(), hpos_new(0), hpos_new(1), &lat_new, &lon_new);
_ekf->global_origin().reproject(hpos_new(0), hpos_new(1), lat_new, lon_new);
_gps_data.lon = static_cast<int32_t>(lon_new * 1e7);
_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);