Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 22:32:11 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

ekf2: require valid filter vz for GPS vspeed check

Browse Source
This commit is contained in:
Daniel Agar
2024-06-21 11:51:55 -04:00
parent 20c0d3a096
commit f832ae688d
2 changed files with 14 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp
+13 -9
View File
@@ -189,15 +189,15 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
// Apply a low pass filter
_gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef);
// Calculate the horizontal drift speed and fail if too high
// 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);
// Fail if the vertical drift speed is too high
// 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);
// Check the magnitude of the filtered horizontal GPS velocity
// 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);
@@ -205,12 +205,20 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
_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();
@@ -223,12 +231,6 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
_gps_pos_prev.initReference(lat, lon, gps.time_us);
_gps_alt_prev = gps.alt;
// Check the filtered difference between GPS and EKF vertical velocity
const float vz_diff_limit = 10.0f * _params.req_vdrift;
const float vertVel = math::constrain(gps.vel(2) - _state.vel(2), -vz_diff_limit, vz_diff_limit);
_gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef);
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);
// assume failed first time through
if (_last_gps_fail_us == 0) {
_last_gps_fail_us = _time_delayed_us;
@@ -260,6 +262,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
void Ekf::resetGpsDriftCheckFilters()
{
_gps_velNE_filt.setZero();
_gps_vel_d_filt = 0.f;
_gps_pos_deriv_filt.setZero();
_gps_horizontal_position_drift_rate_m_s = NAN;
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -652,7 +652,7 @@ private:
Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec)
Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec)
float _gps_velD_diff_filt{0.0f}; ///< GPS filtered Down velocity (m/sec)
float _gps_vel_d_filt{0.0f}; ///< GNSS filtered Down velocity (m/sec)
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time