From 0a5d6a430a476ab3fa145f02add132778b58f047 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 16 Apr 2019 11:36:54 +0900 Subject: [PATCH] AP_NavEKF3: accept optical flow data at up to 50hz --- libraries/AP_NavEKF3/AP_NavEKF3.h | 1 + libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 12 ++++++++++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 51398e00f0..ed15c124eb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -456,6 +456,7 @@ private: const uint8_t gndGradientSigma = 50; // RMS terrain gradient percentage assumed by the terrain height estimation 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) struct { bool enabled:1; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index b6701d2b16..59145351b7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -103,6 +103,9 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) obs_buffer_length = MIN(obs_buffer_length,imu_buffer_length); + // calculate buffer size for optical flow data + const uint8_t flow_buffer_length = MIN((ekf_delay_ms / _frontend->flowIntervalMin_ms) + 1, imu_buffer_length); + if(!storedGPS.init(obs_buffer_length)) { return false; } @@ -115,7 +118,7 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c if(!storedTAS.init(obs_buffer_length)) { return false; } - if(!storedOF.init(obs_buffer_length)) { + if (!storedOF.init(flow_buffer_length)) { return false; } if(!storedBodyOdm.init(obs_buffer_length)) { @@ -138,7 +141,12 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c if(!storedOutput.init(imu_buffer_length)) { return false; } - gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u buffers, IMU=%u , OBS=%u , dt=%6.4f",(unsigned)imu_index,(unsigned)imu_buffer_length,(unsigned)obs_buffer_length,(double)dtEkfAvg); + gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u buffers, IMU=%u, OBS=%u, OF=%u, dt=%6.4f", + (unsigned)imu_index, + (unsigned)imu_buffer_length, + (unsigned)obs_buffer_length, + (unsigned)flow_buffer_length, + (double)dtEkfAvg); return true; }