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

AP_NavEKF3: max delay includes visual odometry

This commit is contained in:
Randy Mackay 2020-07-15 15:02:50 +09:00 committed by Andrew Tridgell
parent 2a3238b7de
commit d8f2d0cf8d
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -6,6 +6,7 @@
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_VisualOdom/AP_VisualOdom.h>
extern const AP_HAL::HAL& hal;
@ -93,6 +94,14 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
maxTimeDelay_ms = MAX(maxTimeDelay_ms , frontend->tasDelay_ms);
}
#if HAL_VISUALODOM_ENABLED
// include delay from visual odometry if enabled
AP_VisualOdom *visual_odom = AP::visualodom();
if ((visual_odom != nullptr) && visual_odom->enabled()) {
maxTimeDelay_ms = MAX(maxTimeDelay_ms, MIN(visual_odom->get_delay_ms(), 250));
}
#endif
// calculate the IMU buffer length required to accommodate the maximum delay with some allowance for jitter
imu_buffer_length = (maxTimeDelay_ms / (uint16_t)(EKF_TARGET_DT_MS)) + 1;