Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: use get_loop_delta_t() from INS

This commit is contained in:
Andrew Tridgell 2015-12-26 14:12:20 +11:00 committed by Randy Mackay
parent 28a684ea03
commit 23cef70846
2 changed files with 5 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -233,7 +233,7 @@ void NavEKF2_core::readIMUData()
const AP_InertialSensor &ins = _ahrs->get_ins(); const AP_InertialSensor &ins = _ahrs->get_ins();
// average IMU sampling rate // average IMU sampling rate
dtIMUavg = 1.0f/ins.get_sample_rate(); dtIMUavg = ins.get_loop_delta_t();
// 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 = AP_HAL::millis(); imuSampleTime_ms = AP_HAL::millis();

13
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -68,16 +68,11 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
than 100Hz is downsampled. For 50Hz main loop rate we need a than 100Hz is downsampled. For 50Hz main loop rate we need a
shorter buffer. shorter buffer.
*/ */
switch (_ahrs->get_ins().get_sample_rate()) { if (_ahrs->get_ins().get_sample_rate() < 100) {
case AP_InertialSensor::RATE_50HZ:
imu_buffer_length = 13; imu_buffer_length = 13;
break; } else {
case AP_InertialSensor::RATE_100HZ:
case AP_InertialSensor::RATE_200HZ:
case AP_InertialSensor::RATE_400HZ:
// maximum 260 msec delay at 100 Hz fusion rate // maximum 260 msec delay at 100 Hz fusion rate
imu_buffer_length = 26; imu_buffer_length = 26;
break;
} }
if(!storedGPS.init(OBS_BUFFER_LENGTH)) { if(!storedGPS.init(OBS_BUFFER_LENGTH)) {
return false; return false;
@ -117,7 +112,7 @@ void NavEKF2_core::InitialiseVariables()
{ {
// calculate the nominal filter update rate // calculate the nominal filter update rate
const AP_InertialSensor &ins = _ahrs->get_ins(); const AP_InertialSensor &ins = _ahrs->get_ins();
localFilterTimeStep_ms = (uint8_t)(1000.0f/(float)ins.get_sample_rate()); localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10); localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10);
// initialise time stamps // initialise time stamps
@ -279,7 +274,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
InitialiseVariables(); InitialiseVariables();
// Initialise IMU data // Initialise IMU data
dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate(); dtIMUavg = _ahrs->get_ins().get_loop_delta_t();
dtEkfAvg = MIN(0.01f,dtIMUavg); dtEkfAvg = MIN(0.01f,dtIMUavg);
readIMUData(); readIMUData();
storedIMU.reset_history(imuDataNew); storedIMU.reset_history(imuDataNew);