Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: accept extnav at up to 50hz

Browse Source
This commit is contained in:
Randy Mackay 2020-06-08 12:13:13 +09:00 committed by Andrew Tridgell
parent 0f212ca727
commit 6c10655059
3 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -894,7 +894,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
{
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if ((timeStamp_ms - extNavMeasTime_ms) < 70) {
if ((timeStamp_ms - extNavMeasTime_ms) < 20) {
return;
} else {
extNavMeasTime_ms = timeStamp_ms;
@ -1018,7 +1018,7 @@ void NavEKF2_core::writeDefaultAirSpeed(float airspeed)
void NavEKF2_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
if ((timeStamp_ms - extNavVelMeasTime_ms) < 70) {
if ((timeStamp_ms - extNavVelMeasTime_ms) < 20) {
return;
}

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -101,7 +101,7 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
if(!storedRangeBeacon.init(imu_buffer_length)) {
return false;
}
if(!storedExtNav.init(OBS_BUFFER_LENGTH)) {
if(!storedExtNav.init(EXTNAV_BUFFER_LENGTH)) {
return false;
}
if(!storedIMU.init(imu_buffer_length)) {
@ -110,7 +110,7 @@ bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
if(!storedOutput.init(imu_buffer_length)) {
return false;
}
if(!storedExtNavVel.init(OBS_BUFFER_LENGTH)) {
if(!storedExtNavVel.init(EXTNAV_BUFFER_LENGTH)) {
return false;
}

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -848,6 +848,7 @@ private:
// Must be larger than the time period defined by IMU_BUFFER_LENGTH
static const uint32_t OBS_BUFFER_LENGTH = 5;
static const uint32_t FLOW_BUFFER_LENGTH = 15;
static const uint32_t EXTNAV_BUFFER_LENGTH = 15;
// Variables
bool statesInitialised; // boolean true when filter states have been initialised