From 3766ee1b60092bcad0ddcb209b3514380b6bffb8 Mon Sep 17 00:00:00 2001 From: chobits Date: Tue, 30 Oct 2018 16:27:51 +0800 Subject: [PATCH] AP_NavEKF2: add external navigation data lag --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 10 ++++++++++ libraries/AP_NavEKF2/AP_NavEKF2.h | 1 + libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 5 +++++ 3 files changed, 16 insertions(+) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 32b85a942c..76811152cc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -544,6 +544,16 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @RebootRequired: True AP_GROUPINFO("OGN_HGT_MASK", 49, NavEKF2, _originHgtMode, 0), + // @Param: EXTNAV_DELAY + // @DisplayName: external navigation system measurement delay (msec) + // @Description: This is the number of msec that the external navigation system measurements lag behind the inertial measurements. + // @Range: 0 127 + // @Increment: 1 + // @User: Advanced + // @Units: ms + // @RebootRequired: True + AP_GROUPINFO("EXTNAV_DELAY", 50, NavEKF2, _extnavDelay_ms, 10), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index ab593dd027..d8a7dd83f6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -395,6 +395,7 @@ private: AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s) AP_Int8 _magMask; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion. AP_Int8 _originHgtMode; // Bitmask controlling post alignment correction and reporting of the EKF origin height. + AP_Int8 _extnavDelay_ms; // effective average delay of external nav system measurements relative to inertial measurements (msec) // Tuning parameters const float gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 465a0787c1..207a41b3e4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -845,6 +845,11 @@ void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &p } extNavDataNew.angErr = angErr; extNavDataNew.body_offset = &sensOffset; + timeStamp_ms = timeStamp_ms - frontend->_extnavDelay_ms; + // Correct for the average intersampling delay due to the filter updaterate + timeStamp_ms -= localFilterTimeStep_ms/2; + // Prevent time delay exceeding age of oldest IMU data in the buffer + timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms); extNavDataNew.time_ms = timeStamp_ms; storedExtNav.push(extNavDataNew);