Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: accept extnav at up to 50hz

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

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -529,6 +529,7 @@ private:
const uint16_t fusionTimeStep_ms = 10; // The minimum time interval between covariance predictions and measurement fusions in msec
const uint8_t sensorIntervalMin_ms = 50; // The minimum allowed time between measurements from any non-IMU sensor (msec)
const uint8_t flowIntervalMin_ms = 20; // The minimum allowed time between measurements from optical flow sensors (msec)
const uint8_t extNavIntervalMin_ms = 20; // The minimum allowed time between measurements from external navigation sensors (msec)
struct {
bool enabled:1;

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -947,7 +947,7 @@ void NavEKF3_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) || !statesInitialised) {
if (((timeStamp_ms - extNavMeasTime_ms) < frontend->extNavIntervalMin_ms) || !statesInitialised) {
return;
} else {
extNavMeasTime_ms = timeStamp_ms;
@ -990,7 +990,7 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
void NavEKF3_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) < frontend->extNavIntervalMin_ms) {
return;
}

12
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -108,6 +108,12 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
// calculate buffer size for optical flow data
const uint8_t flow_buffer_length = MIN((ekf_delay_ms / frontend->flowIntervalMin_ms) + 1, imu_buffer_length);
// calculate buffer size for external nav data
const uint8_t extnav_buffer_length = MIN((ekf_delay_ms / frontend->extNavIntervalMin_ms) + 1, imu_buffer_length);
// buffer size for external yaw
const uint8_t yaw_angle_buffer_length = MAX(obs_buffer_length, extnav_buffer_length);
if(!storedGPS.init(obs_buffer_length)) {
return false;
}
@ -129,7 +135,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
if(!storedWheelOdm.init(imu_buffer_length)) { // initialise to same length of IMU to allow for multiple wheel sensors
return false;
}
if(!storedYawAng.init(obs_buffer_length)) {
if(!storedYawAng.init(yaw_angle_buffer_length)) {
return false;
}
// Note: the use of dual range finders potentially doubles the amount of data to be stored
@ -140,10 +146,10 @@ bool NavEKF3_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 (!storedExtNavVel.init(obs_buffer_length)) {
if (!storedExtNavVel.init(extnav_buffer_length)) {
return false;
}
if(!storedIMU.init(imu_buffer_length)) {