aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: accept optical flow data at up to 50hz

Browse Source
This commit is contained in:
Randy Mackay 2019-04-16 11:36:54 +09:00
parent e19d638c0f
commit 0a5d6a430a
2 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -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;

12
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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;
}