Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 05:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: add external navigation data lag

Browse Source
This commit is contained in:
chobits 2018-10-30 16:27:51 +08:00 committed by Randy Mackay
parent 5485eda199
commit 3766ee1b60
3 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -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
};

1
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -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

5
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -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);