fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 02:34:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: rework amsl to ellipsoid altitude conversion

Browse Source
This commit is contained in:
bresch 2024-09-30 13:30:57 +02:00 committed by Daniel Agar
parent 5bfa6b3359
commit dbf51da99e
2 changed files with 35 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1176,7 +1176,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
#if defined(CONFIG_EKF2_GNSS)
global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
global_pos.alt_ellipsoid = altAmslToEllipsoid(global_pos.alt);
#else
global_pos.alt_ellipsoid = global_pos.alt;
#endif
@ -2051,29 +2051,6 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_GNSS)
float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
{
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
if (_gps_alttitude_ellipsoid_previous_timestamp == 0) {
_wgs84_hgt_offset = height_diff;
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
} else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) {
// apply a 10 second first order low pass filter to baro offset
float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp);
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
_wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f);
}
return amsl_hgt + _wgs84_hgt_offset;
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_AIRSPEED)
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
@ -2433,11 +2410,14 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
return; //TODO: change and set to NAN
}
const float altitude_amsl = static_cast<float>(vehicle_gps_position.altitude_msl_m);
const float altitude_ellipsoid = static_cast<float>(vehicle_gps_position.altitude_ellipsoid_m);
gnssSample gnss_sample{
.time_us = vehicle_gps_position.timestamp,
.lat = vehicle_gps_position.latitude_deg,
.lon = vehicle_gps_position.longitude_deg,
.alt = static_cast<float>(vehicle_gps_position.altitude_msl_m),
.alt = altitude_amsl,
.vel = vel_ned,
.hacc = vehicle_gps_position.eph,
.vacc = vehicle_gps_position.epv,
@ -2454,10 +2434,31 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
_ekf.setGpsData(gnss_sample);
_gps_time_usec = gnss_sample.time_us;
_gps_alttitude_ellipsoid = static_cast<int32_t>(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3));
const float geoid_height = altitude_ellipsoid - altitude_amsl;
if (_last_geoid_height_update_us == 0) {
_geoid_height_lpf.reset(geoid_height);
_last_geoid_height_update_us = gnss_sample.time_us;
} else if (gnss_sample.time_us > _last_geoid_height_update_us) {
const float dt = 1e-6f * (gnss_sample.time_us - _last_geoid_height_update_us);
_geoid_height_lpf.setParameters(dt, kGeoidHeightLpfTimeConstant);
_geoid_height_lpf.update(geoid_height);
_last_geoid_height_update_us = gnss_sample.time_us;
}
}
}
float EKF2::altEllipsoidToAmsl(float ellipsoid_alt) const
{
return ellipsoid_alt - _geoid_height_lpf.getState();
}
float EKF2::altAmslToEllipsoid(float amsl_alt) const
{
return amsl_alt + _geoid_height_lpf.getState();
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)

15
src/modules/ekf2/EKF2.hpp
View File

@ -44,6 +44,7 @@
#include "EKF/ekf.h"
#include "EKF2Selector.hpp"
#include "mathlib/math/filter/AlphaFilter.hpp"
#include <float.h>
@ -210,10 +211,8 @@ private:
bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float filter_altitude_ellipsoid(float amsl_hgt);
float altEllipsoidToAmsl(float ellipsoid_alt) const;
float altAmslToEllipsoid(float amsl_alt) const;
void PublishGpsStatus(const hrt_abstime &timestamp);
void PublishGnssHgtBias(const hrt_abstime &timestamp);
@ -445,10 +444,10 @@ private:
#endif // CONFIG_EKF2_WIND
#if defined(CONFIG_EKF2_GNSS)
uint64_t _gps_time_usec {0};
int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84
uint64_t _last_geoid_height_update_us{0};
static constexpr float kGeoidHeightLpfTimeConstant = 10.f;
AlphaFilter<float> _geoid_height_lpf; ///< height offset between AMSL and ellipsoid
hrt_abstime _last_gps_status_published{0};