Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Correctly initialise dtIMUavg time step and apply LPF

Browse Source
This commit is contained in:
priseborough 2017-04-28 16:26:44 +10:00 committed by Andrew Tridgell
parent 4ce0a8e24e
commit 7abf9997e6
2 changed files with 3 additions and 3 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

@ -302,8 +302,8 @@ void NavEKF3_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
// average IMU sampling rate
dtIMUavg = ins.get_loop_delta_t();
// calculate an averaged IMU update rate using a spike and lowpass filter combination
dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = frontend->imuSampleTime_us / 1000;

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -200,7 +200,7 @@ void NavEKF3_core::InitialiseVariables()
badIMUdata = false;
finalInflightYawInit = false;
finalInflightMagInit = false;
dtIMUavg = 0.0025f;
dtIMUavg = ins.get_loop_delta_t();
dtEkfAvg = EKF_TARGET_DT;
dt = 0;
velDotNEDfilt.zero();