mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 16:08:28 -04:00
AP_NavEKF: Prevent high measurement data rates from overflowing buffers
High measurement data rates can fill buffers with data that is always new and never fused because it is over-written before it falls behind the measurement time horizon.
This commit is contained in:
parent
d1a090dda8
commit
1c347e8859
@ -187,7 +187,9 @@ bool NavEKF2_core::getMagOffsets(Vector3f &magOffsets) const
|
||||
// check for new magnetometer data and update store measurements if available
|
||||
void NavEKF2_core::readMagData()
|
||||
{
|
||||
if (use_compass() && _ahrs->get_compass()->last_update_usec() != lastMagUpdate_us) {
|
||||
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
|
||||
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
|
||||
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
|
||||
// store time of last measurement update
|
||||
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec();
|
||||
|
||||
@ -444,7 +446,8 @@ bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d
|
||||
void NavEKF2_core::readGpsData()
|
||||
{
|
||||
// check for new GPS data
|
||||
if (_ahrs->get_gps().last_message_time_ms() != lastTimeGpsReceived_ms) {
|
||||
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
|
||||
if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > 70) {
|
||||
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
// report GPS fix status
|
||||
gpsCheckStatus.bad_fix = false;
|
||||
@ -704,7 +707,8 @@ void NavEKF2_core::decayGpsOffset()
|
||||
void NavEKF2_core::readHgtData()
|
||||
{
|
||||
// check to see if baro measurement has changed so we know if a new measurement has arrived
|
||||
if (_baro.get_last_update() != lastHgtReceived_ms) {
|
||||
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
|
||||
if (_baro.get_last_update() - lastHgtReceived_ms > 70) {
|
||||
// Don't use Baro height if operating in optical flow mode as we use range finder instead
|
||||
if (frontend._fusionModeGPS == 3 && frontend._altSource == 1) {
|
||||
if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) {
|
||||
|
Loading…
Reference in New Issue
Block a user