mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 12:47:35 +08:00
ekf2: simplify names of scoped variables
This commit is contained in:
bresch
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
c33d79cfb4
commit
0e32b155f3
@@ -40,93 +40,92 @@
|
||||
|
||||
namespace estimator
|
||||
{
|
||||
bool GnssChecks::runGnssChecks(const gnssSample &gnss, uint64_t time_us)
|
||||
bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us)
|
||||
{
|
||||
// assume failed first time through
|
||||
if (_last_gps_fail_us == 0) {
|
||||
_last_gps_fail_us = time_us;
|
||||
if (_time_last_fail_us == 0) {
|
||||
_time_last_fail_us = time_us;
|
||||
}
|
||||
|
||||
bool passed = false;
|
||||
|
||||
if (_initial_checks_passed) {
|
||||
if (runInitialFixChecks(gnss)) {
|
||||
_last_gps_pass_us = time_us;
|
||||
passed = isTimedOut(_last_gps_fail_us, time_us, math::max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10));
|
||||
_time_last_pass_us = time_us;
|
||||
passed = isTimedOut(_time_last_fail_us, time_us, math::max((uint64_t)1e6, (uint64_t)_params.min_health_time_us / 10));
|
||||
|
||||
} else {
|
||||
_last_gps_fail_us = time_us;
|
||||
_time_last_fail_us = time_us;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (runInitialFixChecks(gnss)) {
|
||||
_last_gps_pass_us = time_us;
|
||||
_time_last_pass_us = time_us;
|
||||
|
||||
if (isTimedOut(_last_gps_fail_us, time_us, (uint64_t)_min_gps_health_time_us)) {
|
||||
if (isTimedOut(_time_last_fail_us, time_us, (uint64_t)_params.min_health_time_us)) {
|
||||
_initial_checks_passed = true;
|
||||
passed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
_last_gps_fail_us = time_us;
|
||||
_time_last_fail_us = time_us;
|
||||
}
|
||||
}
|
||||
|
||||
// save GPS fix for next time
|
||||
_gnss_pos_prev.initReference(gnss.lat, gnss.lon, gnss.time_us);
|
||||
_gnss_alt_prev = gnss.alt;
|
||||
lat_lon_prev.initReference(gnss.lat, gnss.lon, gnss.time_us);
|
||||
_alt_prev = gnss.alt;
|
||||
|
||||
_checks_passed = passed;
|
||||
_passed = passed;
|
||||
return passed;
|
||||
}
|
||||
|
||||
bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
|
||||
{
|
||||
// Check the fix type
|
||||
_gps_check_fail_status.flags.fix = (gnss.fix_type < 3);
|
||||
_check_fail_status.flags.fix = (gnss.fix_type < 3);
|
||||
|
||||
// Check the number of satellites
|
||||
_gps_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats);
|
||||
_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats);
|
||||
|
||||
// Check the position dilution of precision
|
||||
_gps_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop);
|
||||
_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop);
|
||||
|
||||
// Check the reported horizontal and vertical position accuracy
|
||||
_gps_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc);
|
||||
_gps_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc);
|
||||
_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc);
|
||||
_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc);
|
||||
|
||||
// Check the reported speed accuracy
|
||||
_gps_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc);
|
||||
_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc);
|
||||
|
||||
_gps_check_fail_status.flags.spoofed = gnss.spoofed;
|
||||
_check_fail_status.flags.spoofed = gnss.spoofed;
|
||||
|
||||
runOnGroundGnssChecks(gnss);
|
||||
|
||||
// force horizontal speed failure if above the limit
|
||||
if (gnss.vel.xy().longerThan(_params.velocity_limit)) {
|
||||
_gps_check_fail_status.flags.hspeed = true;
|
||||
_check_fail_status.flags.hspeed = true;
|
||||
}
|
||||
|
||||
// force vertical speed failure if above the limit
|
||||
if (fabsf(gnss.vel(2)) > _params.velocity_limit) {
|
||||
_gps_check_fail_status.flags.vspeed = true;
|
||||
_check_fail_status.flags.vspeed = true;
|
||||
}
|
||||
|
||||
bool passed = true;
|
||||
|
||||
// if any user selected checks have failed, record the fail time
|
||||
if (
|
||||
_gps_check_fail_status.flags.fix ||
|
||||
(_gps_check_fail_status.flags.nsats && isGnssCheckEnabled(GnssChecksMask::kNsats)) ||
|
||||
(_gps_check_fail_status.flags.pdop && isGnssCheckEnabled(GnssChecksMask::kPdop)) ||
|
||||
(_gps_check_fail_status.flags.hacc && isGnssCheckEnabled(GnssChecksMask::kHacc)) ||
|
||||
(_gps_check_fail_status.flags.vacc && isGnssCheckEnabled(GnssChecksMask::kVacc)) ||
|
||||
(_gps_check_fail_status.flags.sacc && isGnssCheckEnabled(GnssChecksMask::kSacc)) ||
|
||||
(_gps_check_fail_status.flags.hdrift && isGnssCheckEnabled(GnssChecksMask::kHdrift)) ||
|
||||
(_gps_check_fail_status.flags.vdrift && isGnssCheckEnabled(GnssChecksMask::kVdrift)) ||
|
||||
(_gps_check_fail_status.flags.hspeed && isGnssCheckEnabled(GnssChecksMask::kHspd)) ||
|
||||
(_gps_check_fail_status.flags.vspeed && isGnssCheckEnabled(GnssChecksMask::kVspd)) ||
|
||||
(_gps_check_fail_status.flags.spoofed && isGnssCheckEnabled(GnssChecksMask::kSpoofed))
|
||||
_check_fail_status.flags.fix ||
|
||||
(_check_fail_status.flags.nsats && isCheckEnabled(GnssChecksMask::kNsats)) ||
|
||||
(_check_fail_status.flags.pdop && isCheckEnabled(GnssChecksMask::kPdop)) ||
|
||||
(_check_fail_status.flags.hacc && isCheckEnabled(GnssChecksMask::kHacc)) ||
|
||||
(_check_fail_status.flags.vacc && isCheckEnabled(GnssChecksMask::kVacc)) ||
|
||||
(_check_fail_status.flags.sacc && isCheckEnabled(GnssChecksMask::kSacc)) ||
|
||||
(_check_fail_status.flags.hdrift && isCheckEnabled(GnssChecksMask::kHdrift)) ||
|
||||
(_check_fail_status.flags.vdrift && isCheckEnabled(GnssChecksMask::kVdrift)) ||
|
||||
(_check_fail_status.flags.hspeed && isCheckEnabled(GnssChecksMask::kHspd)) ||
|
||||
(_check_fail_status.flags.vspeed && isCheckEnabled(GnssChecksMask::kVspd)) ||
|
||||
(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed))
|
||||
) {
|
||||
passed = false;
|
||||
}
|
||||
@@ -139,12 +138,12 @@ void GnssChecks::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;
|
||||
_check_fail_status.flags.hdrift = false;
|
||||
_check_fail_status.flags.vdrift = false;
|
||||
_check_fail_status.flags.hspeed = false;
|
||||
_check_fail_status.flags.vspeed = false;
|
||||
|
||||
resetGpsDriftCheckFilters();
|
||||
resetDriftFilters();
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -152,7 +151,7 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss)
|
||||
// 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()))
|
||||
lat_lon_prev.getProjectionReferenceTimestamp()))
|
||||
* 1e-6f, 0.001f, filt_time_const);
|
||||
const float filter_coef = dt / filt_time_const;
|
||||
|
||||
@@ -160,64 +159,64 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss)
|
||||
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);
|
||||
// calculate position movement since last fix
|
||||
if (lat_lon_prev.getProjectionReferenceTimestamp() > 0) {
|
||||
lat_lon_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;
|
||||
lat_lon_prev.initReference(gnss.lat, gnss.lon, gnss.time_us);
|
||||
_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));
|
||||
Vector3f delta_pos(delta_pos_n, delta_pos_e, (_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);
|
||||
_lat_lon_alt_deriv_filt = delta_pos / dt * filter_coef + _lat_lon_alt_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);
|
||||
_lat_lon_alt_deriv_filt = matrix::constrain(_lat_lon_alt_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);
|
||||
_horizontal_position_drift_rate_m_s = Vector2f(_lat_lon_alt_deriv_filt.xy()).norm();
|
||||
_check_fail_status.flags.hdrift = (_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);
|
||||
_vertical_position_drift_rate_m_s = fabsf(_lat_lon_alt_deriv_filt(2));
|
||||
_check_fail_status.flags.vdrift = (_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);
|
||||
const Vector2f vel_ne = matrix::constrain(Vector2f(gnss.vel.xy()),
|
||||
-10.0f * _params.req_hdrift,
|
||||
10.0f * _params.req_hdrift);
|
||||
_vel_ne_filt = vel_ne * filter_coef + _vel_ne_filt * (1.0f - filter_coef);
|
||||
_filtered_horizontal_velocity_m_s = _vel_ne_filt.norm();
|
||||
_check_fail_status.flags.hspeed = (_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);
|
||||
_vel_d_filt = gnss_vz * filter_coef + _vel_d_filt * (1.f - filter_coef);
|
||||
|
||||
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift);
|
||||
_check_fail_status.flags.vspeed = (fabsf(_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();
|
||||
resetDriftFilters();
|
||||
}
|
||||
}
|
||||
|
||||
void GnssChecks::resetGpsDriftCheckFilters()
|
||||
void GnssChecks::resetDriftFilters()
|
||||
{
|
||||
_gps_velNE_filt.setZero();
|
||||
_gps_vel_d_filt = 0.f;
|
||||
_vel_ne_filt.setZero();
|
||||
_vel_d_filt = 0.f;
|
||||
|
||||
_gps_pos_deriv_filt.setZero();
|
||||
_lat_lon_alt_deriv_filt.setZero();
|
||||
|
||||
_gps_horizontal_position_drift_rate_m_s = NAN;
|
||||
_gps_vertical_position_drift_rate_m_s = NAN;
|
||||
_gps_filtered_horizontal_velocity_m_s = NAN;
|
||||
_horizontal_position_drift_rate_m_s = NAN;
|
||||
_vertical_position_drift_rate_m_s = NAN;
|
||||
_filtered_horizontal_velocity_m_s = NAN;
|
||||
}
|
||||
}; // namespace estimator
|
||||
|
||||
@@ -46,8 +46,7 @@ public:
|
||||
GnssChecks(int32_t &check_mask, int32_t &req_nsats, float &req_pdop, float &req_hacc, float &req_vacc, float &req_sacc,
|
||||
float &req_hdrift, float &req_vdrift, float &velocity_limit, uint32_t &min_health_time_us,
|
||||
filter_control_status_u &control_status):
|
||||
_params{check_mask, req_nsats, req_pdop, req_hacc, req_vacc, req_sacc, req_hdrift, req_vdrift, velocity_limit},
|
||||
_min_gps_health_time_us(min_health_time_us),
|
||||
_params{check_mask, req_nsats, req_pdop, req_hacc, req_vacc, req_sacc, req_hdrift, req_vdrift, velocity_limit, min_health_time_us},
|
||||
_control_status(control_status)
|
||||
{};
|
||||
|
||||
@@ -76,28 +75,26 @@ public:
|
||||
|
||||
void reset()
|
||||
{
|
||||
_checks_passed = false;
|
||||
_last_gps_pass_us = 0;
|
||||
_last_gps_fail_us = 0;
|
||||
resetGpsDriftCheckFilters();
|
||||
_passed = false;
|
||||
_time_last_pass_us = 0;
|
||||
_time_last_fail_us = 0;
|
||||
resetDriftFilters();
|
||||
}
|
||||
|
||||
/*
|
||||
* Return true if the GPS solution quality is adequate.
|
||||
* Checks are activated using the EKF2_GPS_CHECK bitmask parameter
|
||||
* Checks are adjusted using the EKF2_REQ_* parameters
|
||||
* Return true if the GNSS solution quality is adequate.
|
||||
*/
|
||||
bool runGnssChecks(const gnssSample &gps, uint64_t time_us);
|
||||
bool passed() const { return _checks_passed; }
|
||||
bool run(const gnssSample &gnss, uint64_t time_us);
|
||||
bool passed() const { return _passed; }
|
||||
bool initialChecksPassed() const { return _initial_checks_passed; }
|
||||
uint64_t getLastPassUs() const { return _last_gps_pass_us; }
|
||||
uint64_t getLastFailUs() const { return _last_gps_fail_us; }
|
||||
uint64_t getLastPassUs() const { return _time_last_pass_us; }
|
||||
uint64_t getLastFailUs() const { return _time_last_fail_us; }
|
||||
|
||||
const gps_check_fail_status_u &getFailStatus() const { return _gps_check_fail_status; }
|
||||
const gps_check_fail_status_u &getFailStatus() const { return _check_fail_status; }
|
||||
|
||||
float horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
|
||||
float vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
|
||||
float filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
|
||||
float horizontal_position_drift_rate_m_s() const { return _horizontal_position_drift_rate_m_s; }
|
||||
float vertical_position_drift_rate_m_s() const { return _vertical_position_drift_rate_m_s; }
|
||||
float filtered_horizontal_velocity_m_s() const { return _filtered_horizontal_velocity_m_s; }
|
||||
|
||||
private:
|
||||
enum class GnssChecksMask : int32_t {
|
||||
@@ -113,36 +110,35 @@ private:
|
||||
kSpoofed = (1 << 9)
|
||||
};
|
||||
|
||||
bool isGnssCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast<int32_t>(check)); }
|
||||
bool isCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast<int32_t>(check)); }
|
||||
|
||||
bool runInitialFixChecks(const gnssSample &gnss);
|
||||
void runOnGroundGnssChecks(const gnssSample &gnss);
|
||||
|
||||
void resetGpsDriftCheckFilters();
|
||||
void resetDriftFilters();
|
||||
|
||||
bool isTimedOut(uint64_t timestamp_to_check_us, uint64_t now_us, uint64_t timeout_period) const
|
||||
{
|
||||
return (timestamp_to_check_us == 0) || (timestamp_to_check_us + timeout_period < now_us);
|
||||
}
|
||||
|
||||
gps_check_fail_status_u _gps_check_fail_status{};
|
||||
gps_check_fail_status_u _check_fail_status{};
|
||||
|
||||
float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal 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 _horizontal_position_drift_rate_m_s{NAN};
|
||||
float _vertical_position_drift_rate_m_s{NAN};
|
||||
float _filtered_horizontal_velocity_m_s{NAN};
|
||||
|
||||
MapProjection _gnss_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
|
||||
float _gnss_alt_prev{0.0f}; // height from the previous GPS message (m)
|
||||
MapProjection lat_lon_prev{};
|
||||
float _alt_prev{0.0f};
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
Vector3f _gps_pos_deriv_filt{};
|
||||
Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec)
|
||||
Vector3f _lat_lon_alt_deriv_filt{};
|
||||
Vector2f _vel_ne_filt{};
|
||||
|
||||
float _gps_vel_d_filt{0.0f}; ///< GNSS filtered Down velocity (m/sec)
|
||||
uint64_t _last_gps_fail_us{0};
|
||||
uint64_t _last_gps_pass_us{0};
|
||||
float _vel_d_filt{0.0f}; ///< GNSS filtered Down velocity (m/sec)
|
||||
uint64_t _time_last_fail_us{0};
|
||||
uint64_t _time_last_pass_us{0};
|
||||
bool _initial_checks_passed{false};
|
||||
bool _checks_passed{false};
|
||||
bool _passed{false};
|
||||
|
||||
struct Params {
|
||||
const int32_t &check_mask;
|
||||
@@ -154,10 +150,10 @@ private:
|
||||
const float &req_hdrift;
|
||||
const float &req_vdrift;
|
||||
const float &velocity_limit;
|
||||
const uint32_t &min_health_time_us;
|
||||
};
|
||||
|
||||
const Params _params;
|
||||
const uint32_t &_min_gps_health_time_us; ///< GPS is marked as healthy only after this amount of time
|
||||
const filter_control_status_u &_control_status;
|
||||
};
|
||||
}; // namespace estimator
|
||||
|
||||
@@ -65,7 +65,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
|
||||
const bool initial_checks_passed_prev = _gnss_checks.initialChecksPassed();
|
||||
|
||||
if (_gnss_checks.runGnssChecks(gnss_sample, _time_delayed_us)) {
|
||||
if (_gnss_checks.run(gnss_sample, _time_delayed_us)) {
|
||||
if (_gnss_checks.initialChecksPassed() && !initial_checks_passed_prev) {
|
||||
// First time checks are passing, latching.
|
||||
_information_events.flags.gps_checks_passed = true;
|
||||
|
||||
Reference in New Issue
Block a user