Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

ekf2: move on ground GNSS checks to separate function

Browse Source
This commit is contained in:
bresch
2025-05-21 15:20:29 +02:00
committed by Mathieu Bresciani
parent 66fe3aa2b3
commit 5332010b13
3 changed files with 91 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
+88 -84
View File
@@ -55,112 +55,41 @@
#define MASK_GPS_VSPD (1<<8) #define MASK_GPS_VSPD (1<<8)
#define MASK_GPS_SPOOFED (1<<9) #define MASK_GPS_SPOOFED (1<<9)
bool Ekf::runGnssChecks(const gnssSample &gps) bool Ekf::runGnssChecks(const gnssSample &gnss)
{ {
_gps_check_fail_status.flags.spoofed = gps.spoofed; _gps_check_fail_status.flags.spoofed = gnss.spoofed;
// Check the fix type // Check the fix type
_gps_check_fail_status.flags.fix = (gps.fix_type < 3); _gps_check_fail_status.flags.fix = (gnss.fix_type < 3);
// Check the number of satellites // Check the number of satellites
_gps_check_fail_status.flags.nsats = (gps.nsats < _params.req_nsats); _gps_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats);
// Check the position dilution of precision // Check the position dilution of precision
_gps_check_fail_status.flags.pdop = (gps.pdop > _params.req_pdop); _gps_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop);
// Check the reported horizontal and vertical position accuracy // Check the reported horizontal and vertical position accuracy
_gps_check_fail_status.flags.hacc = (gps.hacc > _params.req_hacc); _gps_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc);
_gps_check_fail_status.flags.vacc = (gps.vacc > _params.req_vacc); _gps_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc);
// Check the reported speed accuracy // Check the reported speed accuracy
_gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc); _gps_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc);
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient runOnGroundGnssChecks(gnss);
constexpr float filt_time_const = 10.0f;
const float dt = math::constrain(float(int64_t(gps.time_us) - 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
const double lat = gps.lat;
const double lon = gps.lon;
if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
// Calculate position movement since last measurement
float delta_pos_n = 0.0f;
float delta_pos_e = 0.0f;
// calculate position movement since last GPS fix
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
_gps_pos_prev.initReference(lat, lon, gps.time_us);
_gps_alt_prev = gps.alt;
}
// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt));
// Apply a low pass filter
_gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
// Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals
_gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit);
// hdrift: calculate the horizontal drift speed and fail if too high
_gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();
_gps_check_fail_status.flags.hdrift = (_gps_horizontal_position_drift_rate_m_s > _params.req_hdrift);
// vdrift: fail if the vertical drift speed is too high
_gps_vertical_position_drift_rate_m_s = fabsf(_gps_pos_deriv_filt(2));
_gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift);
// hspeed: check the magnitude of the filtered horizontal GNSS velocity
const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel.xy()),
-10.0f * _params.req_hdrift,
10.0f * _params.req_hdrift);
_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
_gps_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm();
_gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift);
// vspeed: check the magnitude of the filtered vertical GNSS velocity
const float gnss_vz_limit = 10.f * _params.req_vdrift;
const float gnss_vz = math::constrain(gps.vel(2), -gnss_vz_limit, gnss_vz_limit);
_gps_vel_d_filt = gnss_vz * filter_coef + _gps_vel_d_filt * (1.f - filter_coef);
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift);
} else if (_control_status.flags.in_air) {
// These checks are always declared as passed when flying
// If on ground and moving, the last result before movement commenced is kept
_gps_check_fail_status.flags.hdrift = false;
_gps_check_fail_status.flags.vdrift = false;
_gps_check_fail_status.flags.hspeed = false;
_gps_check_fail_status.flags.vspeed = false;
resetGpsDriftCheckFilters();
} else {
// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
resetGpsDriftCheckFilters();
}
// force horizontal speed failure if above the limit // force horizontal speed failure if above the limit
if (gps.vel.xy().longerThan(_params.velocity_limit)) { if (gnss.vel.xy().longerThan(_params.velocity_limit)) {
_gps_check_fail_status.flags.hspeed = true; _gps_check_fail_status.flags.hspeed = true;
} }
// force vertical speed failure if above the limit // force vertical speed failure if above the limit
if (fabsf(gps.vel(2)) > _params.velocity_limit) { if (fabsf(gnss.vel(2)) > _params.velocity_limit) {
_gps_check_fail_status.flags.vspeed = true; _gps_check_fail_status.flags.vspeed = true;
} }
// save GPS fix for next time // save GPS fix for next time
_gps_pos_prev.initReference(lat, lon, gps.time_us); _gnss_pos_prev.initReference(gnss.lat, gnss.lon, gnss.time_us);
_gps_alt_prev = gps.alt; _gnss_alt_prev = gnss.alt;
// assume failed first time through // assume failed first time through
if (_last_gps_fail_us == 0) { if (_last_gps_fail_us == 0) {
@@ -190,6 +119,81 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
} }
} }
void Ekf::runOnGroundGnssChecks(const gnssSample &gnss)
{
if (_control_status.flags.in_air) {
// These checks are always declared as passed when flying
// If on ground and moving, the last result before movement commenced is kept
_gps_check_fail_status.flags.hdrift = false;
_gps_check_fail_status.flags.vdrift = false;
_gps_check_fail_status.flags.hspeed = false;
_gps_check_fail_status.flags.vspeed = false;
resetGpsDriftCheckFilters();
return;
}
if (_control_status.flags.vehicle_at_rest) {
// 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(gnss.time_us) - int64_t(
_gnss_pos_prev.getProjectionReferenceTimestamp()))
* 1e-6f, 0.001f, filt_time_const);
const float filter_coef = dt / filt_time_const;
// Calculate position movement since last measurement
float delta_pos_n = 0.0f;
float delta_pos_e = 0.0f;
// calculate position movement since last GPS fix
if (_gnss_pos_prev.getProjectionReferenceTimestamp() > 0) {
_gnss_pos_prev.project(gnss.lat, gnss.lon, delta_pos_n, delta_pos_e);
} else {
// no previous position has been set
_gnss_pos_prev.initReference(gnss.lat, gnss.lon, gnss.time_us);
_gnss_alt_prev = gnss.alt;
}
// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gnss_alt_prev - gnss.alt));
// Apply a low pass filter
_gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
// Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals
_gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit);
// hdrift: calculate the horizontal drift speed and fail if too high
_gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm();
_gps_check_fail_status.flags.hdrift = (_gps_horizontal_position_drift_rate_m_s > _params.req_hdrift);
// vdrift: fail if the vertical drift speed is too high
_gps_vertical_position_drift_rate_m_s = fabsf(_gps_pos_deriv_filt(2));
_gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift);
// hspeed: check the magnitude of the filtered horizontal GNSS velocity
const Vector2f gps_velNE = matrix::constrain(Vector2f(gnss.vel.xy()),
-10.0f * _params.req_hdrift,
10.0f * _params.req_hdrift);
_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
_gps_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm();
_gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift);
// vspeed: check the magnitude of the filtered vertical GNSS velocity
const float gnss_vz_limit = 10.f * _params.req_vdrift;
const float gnss_vz = math::constrain(gnss.vel(2), -gnss_vz_limit, gnss_vz_limit);
_gps_vel_d_filt = gnss_vz * filter_coef + _gps_vel_d_filt * (1.f - filter_coef);
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift);
} else {
// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
resetGpsDriftCheckFilters();
}
}
void Ekf::resetGpsDriftCheckFilters() void Ekf::resetGpsDriftCheckFilters()
{ {
_gps_velNE_filt.setZero(); _gps_velNE_filt.setZero();
src/modules/ekf2/EKF/ekf.h
+1
View File
@@ -883,6 +883,7 @@ private:
* Checks are adjusted using the EKF2_REQ_* parameters * Checks are adjusted using the EKF2_REQ_* parameters
*/ */
bool runGnssChecks(const gnssSample &gps); bool runGnssChecks(const gnssSample &gps);
void runOnGroundGnssChecks(const gnssSample &gnss);
void controlGnssHeightFusion(const gnssSample &gps_sample); void controlGnssHeightFusion(const gnssSample &gps_sample);
void stopGpsHgtFusion(); void stopGpsHgtFusion();
src/modules/ekf2/EKF/estimator_interface.h
+2 -2
View File
@@ -402,8 +402,8 @@ protected:
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s) float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s)
MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message MapProjection _gnss_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) float _gnss_alt_prev{0.0f}; // height from the previous GPS message (m)
# if defined(CONFIG_EKF2_GNSS_YAW) # if defined(CONFIG_EKF2_GNSS_YAW)
// innovation consistency check monitoring ratios // innovation consistency check monitoring ratios