fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 17:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Eliminate call to hrt_absolute_time()

Browse Source 
This will make the library more portable
This commit is contained in:
Paul Riseborough
2016-01-27 19:04:31 +11:00
parent 94a6644684
commit 6326433c47
2 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_base.cpp
+3 -3
View File
@@ -171,7 +171,7 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_gps = time_usec;
_last_valid_gps_time_us = hrt_absolute_time();
_last_valid_gps_time_us = _time_last_imu;
gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
@@ -317,7 +317,7 @@ void EstimatorBase::initialiseGPS(struct gps_message *gps)
map_projection_init(&_posRef, lat, lon);
_gps_alt_ref = gps->alt / 1e3f;
_gps_initialised = true;
_last_gps_origin_time_us = hrt_absolute_time();
_last_gps_origin_time_us = _time_last_imu;
}
}
@@ -325,7 +325,7 @@ bool EstimatorBase::position_is_valid()
{
// return true if the position estimate is valid
// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
return _gps_initialised && (hrt_absolute_time() - _last_valid_gps_time_us) < 5e6;
return _gps_initialised && (_time_last_imu - _last_valid_gps_time_us) < 5e6;
}
void EstimatorBase::printStoredIMU()
EKF/gps_checks.cpp
+5 -5
View File
@@ -90,7 +90,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
// Calculate time lapsesd since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
const float filtTimeConst = 10.0f;
float dt = fminf(fmaxf(float(hrt_absolute_time() - _last_gps_origin_time_us)*1e-6f,0.001f),filtTimeConst);
float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us)*1e-6f,0.001f),filtTimeConst);
float filterCoef = dt/filtTimeConst;
// Calculate the horizontal drift velocity components and limit to 10x the threshold
@@ -113,7 +113,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
// Save current position as the reference for next time
map_projection_init(&_posRef, lat, lon);
_last_gps_origin_time_us = hrt_absolute_time();
_last_gps_origin_time_us = _time_last_imu;
// Calculate the vertical drift velocity and limit to 10x the threshold
velLimit = 10.0f * _params.requiredVdrift;
@@ -155,7 +155,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
// assume failed first time through
if (_lastGpsFail_us == 0) {
_lastGpsFail_us = hrt_absolute_time();
_lastGpsFail_us = _time_last_imu;
}
// if any user selected checks have failed, record the fail time
@@ -170,11 +170,11 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps)
(_gpsCheckFailStatus.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
(_gpsCheckFailStatus.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD))
) {
_lastGpsFail_us = hrt_absolute_time();
_lastGpsFail_us = _time_last_imu;
}
// continuous period without fail of 10 seconds required to return a healthy status
if (hrt_absolute_time() - _lastGpsFail_us > 1e7) {
if (_time_last_imu - _lastGpsFail_us > 1e7) {
return true;
}
return false;