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

ekf2: handle all time on delayed horizon (except for newest sample checks)

Browse Source 
- a growing number of samples come into the backend with the time
already delayed (sensor's interrupt setting timestamp sample)
 - if the incoming timestamp is already delayed then the new data checks
(relative to latest IMU) can be slightly wrong
 - handle almost all timestamps and checks on delayed time horizon,
except for explicit checks of new samples
 - isRecent() and isTimedOut() helpers use delayed time
 - add new isNewestSampleRecent() used for checking the incoming
timestamp of the incoming (adjusted) data
This commit is contained in:
Daniel Agar
2022-08-31 08:52:30 -04:00
parent d6a4e158cf
commit 1948c5057a
25 changed files with 193 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/gps_checks.cpp
+8 -9
View File
@@ -66,14 +66,14 @@ bool Ekf::collect_gps(const gpsMessage &gps)
const double lon = gps.lon * 1.0e-7;
if (!_pos_ref.isInitialized()) {
_pos_ref.initReference(lat, lon, _time_last_imu);
_pos_ref.initReference(lat, lon, gps.time_usec);
// 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;
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
_pos_ref.initReference(est_lat, est_lon, _time_last_imu);
_pos_ref.initReference(est_lat, est_lon, gps.time_usec);
}
}
@@ -85,7 +85,6 @@ bool Ekf::collect_gps(const gpsMessage &gps)
_NED_origin_initialised = true;
_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);
@@ -168,7 +167,7 @@ bool Ekf::gps_is_good(const gpsMessage &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.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const);
const float dt = math::constrain(float(int64_t(gps.time_usec) - 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
@@ -186,7 +185,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
} else {
// no previous position has been set
_gps_pos_prev.initReference(lat, lon, _time_last_imu);
_gps_pos_prev.initReference(lat, lon, gps.time_usec);
_gps_alt_prev = 1e-3f * (float)gps.alt;
}
@@ -229,7 +228,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
}
// save GPS fix for next time
_gps_pos_prev.initReference(lat, lon, _time_last_imu);
_gps_pos_prev.initReference(lat, lon, gps.time_usec);
_gps_alt_prev = 1e-3f * (float)gps.alt;
// Check the filtered difference between GPS and EKF vertical velocity
@@ -240,7 +239,7 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
// assume failed first time through
if (_last_gps_fail_us == 0) {
_last_gps_fail_us = _time_last_imu;
_last_gps_fail_us = _imu_sample_delayed.time_us;
}
// if any user selected checks have failed, record the fail time
@@ -256,10 +255,10 @@ bool Ekf::gps_is_good(const gpsMessage &gps)
(_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
(_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD))
) {
_last_gps_fail_us = _time_last_imu;
_last_gps_fail_us = _imu_sample_delayed.time_us;
} else {
_last_gps_pass_us = _time_last_imu;
_last_gps_pass_us = _imu_sample_delayed.time_us;
}
// continuous period without fail of x seconds required to return a healthy status