aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: fix ext nav vel timestamp cal

Browse Source
This commit is contained in:
chobits 2020-08-12 15:01:27 +08:00 committed by Randy Mackay
parent 1c57eed66c
commit 013b39d2a6
1 changed files with 3 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -1036,8 +1036,10 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
return;
}
extNavVelMeasTime_ms = timeStamp_ms - delay_ms;
extNavVelMeasTime_ms = timeStamp_ms;
useExtNavVel = true;
// calculate timestamp
timeStamp_ms = timeStamp_ms - delay_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