From 440d361affabe34b0b1fae51a25e1e318bc171e4 Mon Sep 17 00:00:00 2001 From: priseborough Date: Mon, 4 Dec 2017 19:51:06 +1100 Subject: [PATCH] AP_NavEKF2: Limit range of delta times --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 10 ++++++---- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 95786a58d1..7ecb3e411c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -302,11 +302,10 @@ void NavEKF2_core::readIMUData() // Get delta angle data from primary gyro or primary if not available if (ins.use_gyro(imu_index)) { - readDeltaAngle(imu_index, imuDataNew.delAng); + readDeltaAngle(imu_index, imuDataNew.delAng, imuDataNew.delAngDT); } else { - readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng); + readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng, imuDataNew.delAngDT); } - imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f); // Get current time stamp imuDataNew.time_ms = imuSampleTime_ms; @@ -397,6 +396,7 @@ bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d if (ins_index < ins.get_accel_count()) { ins.get_delta_velocity(ins_index,dVel); dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f); + dVel_dt = MIN(ins.get_delta_velocity_dt(ins_index),1.0e-1f); return true; } return false; @@ -532,12 +532,14 @@ void NavEKF2_core::readGpsData() // read the delta angle and corresponding time interval from the IMU // return false if data is not available -bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { +bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt) { const AP_InertialSensor &ins = _ahrs->get_ins(); if (ins_index < ins.get_gyro_count()) { ins.get_delta_angle(ins_index,dAng); frontend->logging.log_imu = true; + dAng_dt = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f); + dAng_dt = MIN(dt,1.0e-1f); return true; } return false; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 9ba42c6e2f..6c20958d7d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -544,7 +544,7 @@ private: // helper functions for readIMUData bool readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &dVel_dt); - bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng); + bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt); // helper functions for correcting IMU data void correctDeltaAngle(Vector3f &delAng, float delAngDT);