Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -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(); const AP_InertialSensor &ins = _ahrs->get_ins();
// average IMU sampling rate // calculate an averaged IMU update rate using a spike and lowpass filter combination
dtIMUavg = ins.get_loop_delta_t(); 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 // the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = frontend->imuSampleTime_us / 1000; 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; badIMUdata = false;
finalInflightYawInit = false; finalInflightYawInit = false;
finalInflightMagInit = false; finalInflightMagInit = false;
dtIMUavg = 0.0025f; dtIMUavg = ins.get_loop_delta_t();
dtEkfAvg = EKF_TARGET_DT; dtEkfAvg = EKF_TARGET_DT;
dt = 0; dt = 0;
velDotNEDfilt.zero(); velDotNEDfilt.zero();