ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

#include <AP_HAL/AP_HAL.h>

#include "AP_NavEKF3_core.h"
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_DAL/AP_DAL.h>
#include <AP_InternalError/AP_InternalError.h>

#if AP_RANGEFINDER_ENABLED
/********************************************************
*              OPT FLOW AND RANGE FINDER                *
********************************************************/

// Read the range finder and take new measurements if available
// Apply a median filter
void NavEKF3_core::readRangeFinder(void)
{
    uint8_t midIndex;
    uint8_t maxIndex;
    uint8_t minIndex;
    // get theoretical correct range when the vehicle is on the ground
    // don't allow range to go below 5cm because this can cause problems with optical flow processing
    const auto *_rng = dal.rangefinder();
    if (_rng == nullptr) {
        return;
    }
    rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f);

    // limit update rate to maximum allowed by data buffers
    if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) {

        // reset the timer used to control the measurement rate
        lastRngMeasTime_ms =  imuSampleTime_ms;

        // store samples and sample time into a ring buffer if valid
        // use data from two range finders if available

        for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
            const auto *sensor = _rng->get_backend(sensorIndex);
            if (sensor == nullptr) {
                continue;
            }
            if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
                rngMeasIndex[sensorIndex] ++;
                if (rngMeasIndex[sensorIndex] > 2) {
                    rngMeasIndex[sensorIndex] = 0;
                }
                storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
                storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f;
            } else {
                continue;
            }

            // check for three fresh samples
            bool sampleFresh[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3] = {};
            for (uint8_t index = 0; index <= 2; index++) {
                sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500;
            }

            // find the median value if we have three fresh samples
            if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) {
                if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) {
                    minIndex = 1;
                    maxIndex = 0;
                } else {
                    minIndex = 0;
                    maxIndex = 1;
                }
                if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) {
                    midIndex = maxIndex;
                } else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) {
                    midIndex = minIndex;
                } else {
                    midIndex = 2;
                }

                // don't allow time to go backwards
                if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) {
                    rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex];
                }

                // limit the measured range to be no less than the on-ground range
                rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);

                // get position in body frame for the current sensor
                rangeDataNew.sensor_idx = sensorIndex;

                // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
                storedRange.push(rangeDataNew);

                // indicate we have updated the measurement
                rngValidMeaTime_ms = imuSampleTime_ms;

            } else if (onGround && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
                // before takeoff we assume on-ground range value if there is no data
                rangeDataNew.time_ms = imuSampleTime_ms;
                rangeDataNew.rng = rngOnGnd;

                // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
                storedRange.push(rangeDataNew);

                // indicate we have updated the measurement
                rngValidMeaTime_ms = imuSampleTime_ms;

            }
        }
    }
}
#endif  // AP_RANGEFINDER_ENABLED

void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{
#if EK3_FEATURE_BODY_ODOM
    // protect against NaN
    if (isnan(quality) || delPos.is_nan() || delAng.is_nan() || isnan(delTime) || posOffset.is_nan()) {
        return;
    }

    // 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 - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {
        return;
    }

    // subtract delay from timestamp
    timeStamp_ms -= delay_ms;

    bodyOdmDataNew.body_offset = posOffset.toftype();
    bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime);
    bodyOdmDataNew.time_ms = timeStamp_ms;
    bodyOdmDataNew.angRate = (delAng * (1.0/delTime)).toftype();
    bodyOdmMeasTime_ms = timeStamp_ms;

    // simple model of accuracy
    // TODO move this calculation outside of EKF into the sensor driver
    bodyOdmDataNew.velErr = frontend->_visOdmVelErrMin + (frontend->_visOdmVelErrMax - frontend->_visOdmVelErrMin) * (1.0f - 0.01f * quality);

    storedBodyOdm.push(bodyOdmDataNew);
#endif // EK3_FEATURE_BODY_ODOM
}

void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
#if EK3_FEATURE_BODY_ODOM
    // This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units
    // It uses the exisiting body frame velocity fusion.
    // TODO implement a dedicated wheel odometry observation model

    // rate limiting to 50hz should be done by the caller
    // 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 ((delTime < dtEkfAvg) || !statesInitialised) {
        return;
    }

    wheel_odm_elements wheelOdmDataNew = {};
    wheelOdmDataNew.hub_offset = posOffset.toftype();
    wheelOdmDataNew.delAng = delAng;
    wheelOdmDataNew.radius = radius;
    wheelOdmDataNew.delTime = delTime;

    // because we are currently converting to an equivalent velocity measurement before fusing
    // the measurement time is moved back to the middle of the sampling period
    wheelOdmDataNew.time_ms = timeStamp_ms - (uint32_t)(500.0f * delTime);

    storedWheelOdm.push(wheelOdmDataNew);
#endif // EK3_FEATURE_BODY_ODOM
}

// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
    // limit update rate to maximum allowed by sensor buffers
    if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) {
        return;
    }

    // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
    // The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
    // A positive X rate is produced by a positive sensor rotation about the X axis
    // A positive Y rate is produced by a positive sensor rotation about the Y axis
    // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
    // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
    flowMeaTime_ms = imuSampleTime_ms;
    // calculate bias errors on flow sensor gyro rates, but protect against spikes in data
    // reset the accumulated body delta angle and time
    // don't do the calculation if not enough time lapsed for a reliable body rate measurement
    if (delTimeOF > 0.01f) {
        flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_ftype((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
        flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_ftype((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
        delAngBodyOF.zero();
        delTimeOF = 0.0f;
    }
    // by definition if this function is called, then flow measurements have been provided so we
    // need to run the optical flow takeoff detection
    detectOptFlowTakeoff();

    // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
    if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
        // correct flow sensor body rates for bias and write
        of_elements ofDataNew {};
        ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
        ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
        // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
        if (delTimeOF > 0.001f) {
            // first preference is to use the rate averaged over the same sampling period as the flow sensor
            ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
        } else if (imuDataNew.delAngDT > 0.001f){
            // second preference is to use most recent IMU data
            ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
        } else {
            // third preference is use zero
            ofDataNew.bodyRadXYZ.z =  0.0f;
        }
        // write uncorrected flow rate measurements
        // note correction for different axis and sign conventions used by the px4flow sensor
        ofDataNew.flowRadXY = - rawFlowRates.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
        // write the flow sensor position in body frame
        ofDataNew.body_offset = posOffset.toftype();
        // write the flow sensor height override
        ofDataNew.heightOverride = heightOverride;
        // write flow rate measurements corrected for body rates
        ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
        ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
        // record time last observation was received so we can detect loss of data elsewhere
        flowValidMeaTime_ms = imuSampleTime_ms;
        // estimate sample time of the measurement
        ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
        // Correct for the average intersampling delay due to the filter updaterate
        ofDataNew.time_ms -= localFilterTimeStep_ms/2;
        // Prevent time delay exceeding age of oldest IMU data in the buffer
        ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
        // Save data to buffer
        storedOF.push(ofDataNew);
    }
}


/********************************************************
*                      MAGNETOMETER                     *
********************************************************/

// try changing compass, return true if a new compass is found
void NavEKF3_core::tryChangeCompass(void)
{
    const auto &compass = dal.compass();
    const uint8_t maxCount = compass.get_count();

    // search through the list of magnetometers
    for (uint8_t i=1; i<maxCount; i++) {
        uint8_t tempIndex = magSelectIndex + i;
        // loop back to the start index if we have exceeded the bounds
        if (tempIndex >= maxCount) {
            tempIndex -= maxCount;
        }
        // if the magnetometer is allowed to be used for yaw and has a different index, we start using it
        if (compass.healthy(tempIndex) && compass.use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
            magSelectIndex = tempIndex;
            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
            // reset the timeout flag and timer
            magTimeout = false;
            lastHealthyMagTime_ms = imuSampleTime_ms;
            // zero the learned magnetometer bias states
            stateStruct.body_magfield.zero();
            // clear the measurement buffer
            storedMag.reset();
            // clear the data waiting flag so that we do not use any data pending from the previous sensor
            magDataToFuse = false;
            // request a reset of the magnetic field states
            magStateResetRequest = true;
            // declare the field unlearned so that the reset request will be obeyed
            magFieldLearned = false;
            // reset body mag variances on next CovariancePrediction
            needMagBodyVarReset = true;
            return;
        }
    }
}

// check for new magnetometer data and update store measurements if available
void NavEKF3_core::readMagData()
{
    const auto &compass = dal.compass();

    if (!compass.available()) {
        allMagSensorsFailed = true;
        return;        
    }

    // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
    // magnetometer, then declare the magnetometers as failed for this flight
    const uint8_t maxCount = compass.get_count();
    if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
        allMagSensorsFailed = true;
        return;
    }

    if (compass.learn_offsets_enabled()) {
        wasLearningCompass_ms = imuSampleTime_ms;
    } else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) {
        // allow time for old data to clear the buffer before signalling other code that compass data can be used
        wasLearningCompass_ms = 0;
    }

    // If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available
    // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
    // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
    // if the timeout is due to a sensor failure, then declare a timeout regardless of onground status
    if (maxCount > 1) {
        bool fusionTimeout = magTimeout && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000 && !(frontend->_affinity & EKF_AFFINITY_MAG);
        bool sensorTimeout = !compass.healthy(magSelectIndex) && imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms;
        if (fusionTimeout || sensorTimeout) {
            tryChangeCompass();
        }
    }

    // limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
    if (use_compass() &&
        compass.healthy(magSelectIndex) &&
        ((compass.last_update_usec(magSelectIndex) - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {

        // detect changes to magnetometer offset parameters and reset states
        Vector3F nowMagOffsets = compass.get_offsets(magSelectIndex).toftype();
        bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
        if (changeDetected) {
            // zero the learned magnetometer bias states
            stateStruct.body_magfield.zero();
            // clear the measurement buffer
            storedMag.reset();

            // reset body mag variances on next
            // CovariancePrediction. This copes with possible errors
            // in the new offsets
            needMagBodyVarReset = true;
        }
        lastMagOffsets = nowMagOffsets;
        lastMagOffsetsValid = true;

        // store time of last measurement update
        lastMagUpdate_us = compass.last_update_usec(magSelectIndex);

        // Magnetometer data at the current time horizon
        mag_elements magDataNew;

        // estimate of time magnetometer measurement was taken, allowing for delays
        magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;

        // Correct for the average intersampling delay due to the filter updaterate
        magDataNew.time_ms -= localFilterTimeStep_ms/2;

        // read compass data and scale to improve numerical conditioning
        magDataNew.mag = (compass.get_field(magSelectIndex) * 0.001f).toftype();

        // check for consistent data between magnetometers
        consistentMagData = compass.consistent();

        // save magnetometer measurement to buffer to be fused later
        storedMag.push(magDataNew);

        // remember time we read compass, to detect compass sensor failure
        lastMagRead_ms = imuSampleTime_ms;
    }
}

/********************************************************
*                Inertial Measurements                  *
********************************************************/

/*
 *  Read IMU delta angle and delta velocity measurements and downsample to 100Hz
 *  for storage in the data buffers used by the EKF. If the IMU data arrives at
 *  lower rate than 100Hz, then no downsampling or upsampling will be performed.
 *  Downsampling is done using a method that does not introduce coning or sculling
 *  errors.
 */
void NavEKF3_core::readIMUData()
{
    const auto &ins = dal.ins();

    // calculate an averaged IMU update rate using a spike and lowpass filter combination
    dtIMUavg = 0.02f * constrain_ftype(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;

    // the imu sample time is used as a common time reference throughout the filter
    imuSampleTime_ms = frontend->imuSampleTime_us / 1000;

    uint8_t accel_active, gyro_active;

    if (ins.use_accel(imu_index)) {
        accel_active = imu_index;
    } else {
        accel_active = ins.get_first_usable_accel();
    }

    if (ins.use_gyro(imu_index)) {
        gyro_active = imu_index;
    } else {
        gyro_active = ins.get_first_usable_gyro();
    }

    if (gyro_active != gyro_index_active) {
        // we are switching active gyro at runtime. Copy over the
        // bias we have learned from the previously inactive
        // gyro. We don't re-init the bias uncertainty as it should
        // have the same uncertainty as the previously active gyro
        stateStruct.gyro_bias = inactiveBias[gyro_active].gyro_bias;
        gyro_index_active = gyro_active;
    }

    if (accel_active != accel_index_active) {
        // switch to the learned accel bias for this IMU
        stateStruct.accel_bias = inactiveBias[accel_active].accel_bias;
        accel_index_active = accel_active;
    }

    // update the inactive bias states
    learnInactiveBiases();

    // run movement check using IMU data
    updateMovementCheck();

    readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
    accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype();
    imuDataNew.accel_index = accel_index_active;
    
    // Get delta angle data from primary gyro or primary if not available
    readDeltaAngle(gyro_index_active, imuDataNew.delAng, imuDataNew.delAngDT);
    imuDataNew.delAngDT = MAX(imuDataNew.delAngDT, 1.0e-4f);
    imuDataNew.gyro_index = gyro_index_active;

    // Get current time stamp
    imuDataNew.time_ms = imuSampleTime_ms;

    // Accumulate the measurement time interval for the delta velocity and angle data
    imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
    imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;

    // use the most recent IMU index for the downsampled IMU
    // data. This isn't strictly correct if we switch IMUs between
    // samples
    imuDataDownSampledNew.gyro_index = imuDataNew.gyro_index;
    imuDataDownSampledNew.accel_index = imuDataNew.accel_index;

    // Rotate quaternon atitude from previous to new and normalise.
    // Accumulation using quaternions prevents introduction of coning errors due to downsampling
    imuQuatDownSampleNew.rotate(imuDataNew.delAng);
    imuQuatDownSampleNew.normalize();

    // Rotate the latest delta velocity into body frame at the start of accumulation
    Matrix3F deltaRotMat;
    imuQuatDownSampleNew.rotation_matrix(deltaRotMat);

    // Apply the delta velocity to the delta velocity accumulator
    imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;

    // Keep track of the number of IMU frames since the last state prediction
    framesSincePredict++;

    /*
     * If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle,
     * then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more
     * than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the
     * IMU data.
     */
    if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5f)) && startPredictEnabled) ||
        (imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) {

        // convert the accumulated quaternion to an equivalent delta angle
        imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);

        // Time stamp the data
        imuDataDownSampledNew.time_ms = imuSampleTime_ms;

        // Write data to the FIFO IMU buffer
        storedIMU.push_youngest_element(imuDataDownSampledNew);

        // calculate the achieved average time step rate for the EKF using a combination spike and LPF
        ftype dtNow = constrain_ftype(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
        dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;

        // do an addtional down sampling for data used to sample XY body frame drag specific forces
        SampleDragData(imuDataDownSampledNew);

        // zero the accumulated IMU data and quaternion
        imuDataDownSampledNew.delAng.zero();
        imuDataDownSampledNew.delVel.zero();
        imuDataDownSampledNew.delAngDT = 0.0f;
        imuDataDownSampledNew.delVelDT = 0.0f;
        imuQuatDownSampleNew[0] = 1.0f;
        imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;

        // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
        framesSincePredict = 0;

        // set the flag to let the filter know it has new IMU data and needs to run
        runUpdates = true;

        // extract the oldest available data from the FIFO buffer
        imuDataDelayed = storedIMU.get_oldest_element();

        // protect against delta time going to zero
        ftype minDT = 0.1f * dtEkfAvg;
        imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
        imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);

        updateTimingStatistics();
        
        // correct the extracted IMU data for sensor errors
        delAngCorrected = imuDataDelayed.delAng;
        delVelCorrected = imuDataDelayed.delVel;
        correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT, imuDataDelayed.gyro_index);
        correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT, imuDataDelayed.accel_index);

    } else {
        // we don't have new IMU data in the buffer so don't run filter updates on this time step
        runUpdates = false;
    }
}

// read the delta velocity and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt) {
    const auto &ins = dal.ins();

    if (ins_index < ins.get_accel_count()) {
        Vector3f dVelF;
        float dVel_dtF;
        ins.get_delta_velocity(ins_index, dVelF, dVel_dtF);
        dVel = dVelF.toftype();
        dVel_dt = dVel_dtF;
        dVel_dt = MAX(dVel_dt,1.0e-4);
        return true;
    }
    return false;
}

/********************************************************
*             Global Position Measurement               *
********************************************************/

// check for new valid GPS data and update stored measurement if available
void NavEKF3_core::readGpsData()
{
    // check for new GPS data
    const auto &gps = dal.gps();

    // limit update rate to avoid overflowing the FIFO buffer
    if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
        return;
    }

    if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
        // report GPS fix status
        gpsCheckStatus.bad_fix = true;
        dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
        return;
    }

    // report GPS fix status
    gpsCheckStatus.bad_fix = false;

    // store fix time from previous read
    const uint32_t secondLastGpsTime_ms = lastTimeGpsReceived_ms;

    // get current fix time
    lastTimeGpsReceived_ms = gps.last_message_time_ms(selected_gps);

    // estimate when the GPS fix was valid, allowing for GPS processing and other delays
    // ideally we should be using a timing signal from the GPS receiver to set this time
    // Use the driver specified delay
    float gps_delay_sec = 0;
    gps.get_lag(selected_gps, gps_delay_sec);
    gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f);

    // Correct for the average intersampling delay due to the filter updaterate
    gpsDataNew.time_ms -= localFilterTimeStep_ms/2;

    // Prevent the time stamp falling outside the oldest and newest IMU data in the buffer
    gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms);

    // Get which GPS we are using for position information
    gpsDataNew.sensor_idx = selected_gps;

    // read the NED velocity from the GPS
    gpsDataNew.vel = gps.velocity(selected_gps).toftype();
    gpsDataNew.have_vz = gps.have_vertical_velocity(selected_gps);

    // position and velocity are not yet corrected for sensor position
    gpsDataNew.corrected = false;

    // Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
    // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
    ftype alpha = constrain_ftype(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
    gpsSpdAccuracy *= (1.0f - alpha);
    float gpsSpdAccRaw;
    if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) {
        gpsSpdAccuracy = 0.0f;
    } else {
        gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
        gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
        gpsSpdAccuracy = MAX(gpsSpdAccuracy,frontend->_gpsHorizVelNoise);
    }
    gpsPosAccuracy *= (1.0f - alpha);
    float gpsPosAccRaw;
    if (!gps.horizontal_accuracy(selected_gps, gpsPosAccRaw)) {
        gpsPosAccuracy = 0.0f;
    } else {
        gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
        gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f);
        gpsPosAccuracy = MAX(gpsPosAccuracy, frontend->_gpsHorizPosNoise);
    }
    gpsHgtAccuracy *= (1.0f - alpha);
    float gpsHgtAccRaw;
    if (!gps.vertical_accuracy(selected_gps, gpsHgtAccRaw)) {
        gpsHgtAccuracy = 0.0f;
    } else {
        gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
        gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f);
        gpsHgtAccuracy = MAX(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise);
    }

    // check if we have enough GPS satellites and increase the gps noise scaler if we don't
    if (gps.num_sats(selected_gps) >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
        gpsNoiseScaler = 1.0f;
    } else if (gps.num_sats(selected_gps) == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
        gpsNoiseScaler = 1.4f;
    } else { // <= 4 satellites or in constant position mode
        gpsNoiseScaler = 2.0f;
    }

    // Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
    if (gpsDataNew.have_vz && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS)) {
        useGpsVertVel = true;
    } else {
        useGpsVertVel = false;
    }

    if ((frontend->_options & (int32_t)NavEKF3::Options::JammingExpected) &&
        (lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) {
        const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000);
        const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);;
        const bool canDoWindRelNav = assume_zero_sideslip();
        const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || canDoWindRelNav || doingBodyVelNav);
        if (canDeadReckon) {
            // If we can do dead reckoning with a data source other than GPS there is time to wait
            // for GPS alignment checks to pass before using GPS inside the EKF.
            // this gets set back to false in calcGpsGoodToAlign() when GPS checks pass
            waitingForGpsChecks = true;
            // force GPS checks to restart
            lastPreAlignGpsCheckTime_ms = 0;
            lastGpsVelFail_ms = imuSampleTime_ms;
            lastGpsVelPass_ms = 0;
            gpsGoodToAlign = false;
        } else {
            waitingForGpsChecks = false;
        }
    }

    // Monitor quality of the GPS velocity data before and after alignment
    calcGpsGoodToAlign();

    // Post-alignment checks
    calcGpsGoodForFlight();

    // Read the GPS location in WGS-84 lat,long,height coordinates
    const Location &gpsloc = gps.location(selected_gps);

    // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
    if (gpsGoodToAlign && !validOrigin) {
        Location gpsloc_fieldelevation = gpsloc; 
        // if flying, correct for height change from takeoff so that the origin is at field elevation
        if (inFlight) {
            gpsloc_fieldelevation.alt += (int32_t)(100.0f * stateStruct.position.z);
        }

        if (!setOrigin(gpsloc_fieldelevation)) {
            // set an error as an attempt was made to set the origin more than once
            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            return;
        }

        // set the NE earth magnetic field states using the published declination
        // and set the corresponding variances and covariances
        alignMagStateDeclination();

        // Set the height of the NED origin
        ekfGpsRefHgt = (double)0.01 * (double)gpsloc.alt + (double)outputDataNew.position.z;

        // Set the uncertainty of the GPS origin height
        ekfOriginHgtVar = sq(gpsHgtAccuracy);

    }

    if (gpsGoodToAlign && !have_table_earth_field) {
        const auto &compass = dal.compass();
        if (compass.have_scale_factor(magSelectIndex) &&
            compass.auto_declination_enabled()) {
            getEarthFieldTable(gpsloc);
            if (frontend->_mag_ef_limit > 0) {
                // initialise earth field from tables
                stateStruct.earth_magfield = table_earth_field_ga;
            }
        }
    }

    // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
    // and are not waiting for GPs checks to pass
    if (validOrigin && !waitingForGpsChecks) {
        gpsDataNew.lat = gpsloc.lat;
        gpsDataNew.lng = gpsloc.lng;
        if ((frontend->_originHgtMode & (1<<2)) == 0) {
            // the height adjustment to match GPS is being achieved by adjusting the origin height
            gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
        } else {
            // the height adjustment to match GPS is being achieved by adjusting the measurements
            gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
        }
        storedGPS.push(gpsDataNew);
        // declare GPS in use
        gpsIsInUse = true;
    }
}

// check for new valid GPS yaw data
void NavEKF3_core::readGpsYawData()
{
    const auto &gps = dal.gps();

    // if the GPS has yaw data then fuse it as an Euler yaw angle
    float yaw_deg, yaw_accuracy_deg;
    uint32_t yaw_time_ms;
    if (gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D &&
        dal.gps().gps_yaw_deg(selected_gps, yaw_deg, yaw_accuracy_deg, yaw_time_ms) &&
        yaw_time_ms != yawMeasTime_ms) {
        // GPS modules are rather too optimistic about their
        // accuracy. Set to min of 5 degrees here to prevent
        // the user constantly receiving warnings about high
        // normalised yaw innovations
        const ftype min_yaw_accuracy_deg = 5.0f;
        yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
        writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), yaw_time_ms, 2);
    }
}

// read the delta angle and corresponding time interval from the IMU
// return false if data is not available
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAngDT) {
    const auto &ins = dal.ins();

    if (ins_index < ins.get_gyro_count()) {
        Vector3f dAngF;
        float dAngDTF;
        ins.get_delta_angle(ins_index, dAngF, dAngDTF);
        dAng = dAngF.toftype();
        dAngDT = dAngDTF;
        return true;
    }
    return false;
}


/********************************************************
*                  Height Measurements                  *
********************************************************/

// check for new pressure altitude measurement data and update stored measurement if available
void NavEKF3_core::readBaroData()
{
    // check to see if baro measurement has changed so we know if a new measurement has arrived
    // limit update rate to avoid overflowing the FIFO buffer
    const auto &baro = dal.baro();
    if (baro.get_last_update(selected_baro) - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) {

        baroDataNew.hgt = baro.get_altitude(selected_baro);

        // time stamp used to check for new measurement
        lastBaroReceived_ms = baro.get_last_update(selected_baro);

        // estimate of time height measurement was taken, allowing for delays
        baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;

        // Correct for the average intersampling delay due to the filter updaterate
        baroDataNew.time_ms -= localFilterTimeStep_ms/2;

        // Prevent time delay exceeding age of oldest IMU data in the buffer
        baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms);

        // save baro measurement to buffer to be fused later
        storedBaro.push(baroDataNew);
    }
}

// calculate filtered offset between baro height measurement and EKF height estimate
// offset should be subtracted from baro measurement to match filter estimate
// offset is used to enable reversion to baro from alternate height data source
void NavEKF3_core::calcFiltBaroOffset()
{
    // Apply a first order LPF with spike protection
    baroHgtOffset += 0.1f * constrain_ftype(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
}

// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
void NavEKF3_core::correctEkfOriginHeight()
{
    // Estimate the WGS-84 height of the EKF's origin using a Bayes filter

    // calculate the variance of our a-priori estimate of the ekf origin height
    ftype deltaTime = constrain_ftype(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0, 1.0);
    if (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
        // Use the baro drift rate
        const ftype baroDriftRate = 0.05;
        ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
    } else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) {
        // use the worse case expected terrain gradient and vehicle horizontal speed
        const ftype maxTerrGrad = 0.25;
        ekfOriginHgtVar += sq(maxTerrGrad * stateStruct.velocity.xy().length() * deltaTime);
    } else {
        // by definition our height source is absolute so cannot run this filter
        return;
    }
    lastOriginHgtTime_ms = imuDataDelayed.time_ms;

    // calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
    // when not using GPS as height source
    ftype originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9];

    // calculate the correction gain
    ftype gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);

    // calculate the innovation
    ftype innovation = - stateStruct.position.z - gpsDataDelayed.hgt;

    // check the innovation variance ratio
    ftype ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);

    // correct the EKF origin and variance estimate if the innovation is less than 5-sigma
    if (ratio < 25.0f && gpsAccuracyGood) {
        ekfGpsRefHgt -= (double)(gain * innovation);
        ekfOriginHgtVar -= MAX(gain * ekfOriginHgtVar , 0.0f);
    }
}

/********************************************************
*                Air Speed Measurements                 *
********************************************************/

// check for new airspeed data and update stored measurements if available
void NavEKF3_core::readAirSpdData()
{
    const float EAS2TAS = dal.get_EAS2TAS();
    // if airspeed reading is valid and is set by the user to be used and has been updated then
    // we take a new reading, convert from EAS to TAS and set the flag letting other functions
    // know a new measurement is available

    if (useAirspeed()) {
        const auto *airspeed = dal.airspeed();
        if (airspeed &&
            (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
            tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed);
            if (tasDataNew.allowFusion) {
                tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS;
                timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
                tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
                tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f));
                // Correct for the average intersampling delay due to the filter update rate
                tasDataNew.time_ms -= localFilterTimeStep_ms/2;
                // Save data into the buffer to be fused when the fusion time horizon catches up with it
                storedTAS.push(tasDataNew);
            }
        }
        // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
        tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
    } else {
        if (is_positive(defaultAirSpeed)) {
            // this is the preferred method with the autopilot providing a model based airspeed estimate
            if (imuDataDelayed.time_ms - prevTasStep_ms > 200 ) {
                tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
                tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f)));
                tasDataToFuse = true;
                tasDataDelayed.allowFusion = true;
                tasDataDelayed.time_ms = imuDataDelayed.time_ms;
            } else {
                tasDataToFuse = false;
                tasDataDelayed.allowFusion = false;
            }
        } else if (lastAspdEstIsValid && !windStateIsObservable) {
            // this uses the last airspeed estimated before dead reckoning started and
            // wind states became unobservable
            if (lastAspdEstIsValid && imuDataDelayed.time_ms - prevTasStep_ms > 200) {
                tasDataDelayed.tas = lastAirspeedEstimate;
                // this airspeed estimate has a lot of uncertainty
                tasDataDelayed.tasVariance = sq(MAX(MAX(frontend->_easNoise, 0.5f), 0.5f * lastAirspeedEstimate));
                tasDataToFuse = true;
                tasDataDelayed.allowFusion = true;
                tasDataDelayed.time_ms = imuDataDelayed.time_ms;
            } else {
                tasDataToFuse = false;
                tasDataDelayed.allowFusion = false;
            }
        }
    }
}

#if EK3_FEATURE_BEACON_FUSION
/********************************************************
*              Range Beacon Measurements                *
********************************************************/

// check for new range beacon data and push to data buffer if available
void NavEKF3_core::readRngBcnData()
{
    // check that arrays are large enough
    static_assert(ARRAY_SIZE(rngBcn.lastTime_ms) >= AP_BEACON_MAX_BEACONS, "lastTimeRngBcn_ms should have at least AP_BEACON_MAX_BEACONS elements");

    // get the location of the beacon data
    const AP_DAL_Beacon *beacon = dal.beacon();

    // exit immediately if no beacon object
    if (beacon == nullptr) {
        return;
    }

    // get the number of beacons in use
    rngBcn.N = MIN(beacon->count(), ARRAY_SIZE(rngBcn.lastTime_ms));

    // search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
    bool newDataPushed = false;
    uint8_t numRngBcnsChecked = 0;
    // start the search one index up from where we left it last time
    uint8_t index = rngBcn.lastChecked;
    while (!newDataPushed && (numRngBcnsChecked < rngBcn.N)) {
        // track the number of beacons checked
        numRngBcnsChecked++;

        // move to next beacon, wrap index if necessary
        index++;
        if (index >= rngBcn.N) {
            index = 0;
        }

        // check that the beacon is healthy and has new data
        if (beacon->beacon_healthy(index) && beacon->beacon_last_update_ms(index) != rngBcn.lastTime_ms[index]) {
            rng_bcn_elements rngBcnDataNew = {};

            // set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
            rngBcn.lastTime_ms[index] = beacon->beacon_last_update_ms(index);
            rngBcnDataNew.time_ms = rngBcn.lastTime_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;

            // set the range noise
            // TODO the range library should provide the noise/accuracy estimate for each beacon
            rngBcnDataNew.rngErr = frontend->_rngBcnNoise;

            // set the range measurement
            rngBcnDataNew.rng = beacon->beacon_distance(index);

            // set the beacon position
            rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();

            // identify the beacon identifier
            rngBcnDataNew.beacon_ID = index;

            // indicate we have new data to push to the buffer
            newDataPushed = true;

            // update the last checked index
            rngBcn.lastChecked = index;

            // Save data into the buffer to be fused when the fusion time horizon catches up with it
            rngBcn.storedRange.push(rngBcnDataNew);
        }
    }

    // Check if the beacon system has returned a 3D fix
    Vector3f bp;
    float bperr;
    if (beacon->get_vehicle_position_ned(bp, bperr)) {
        rngBcn.last3DmeasTime_ms = imuSampleTime_ms;
    }
    rngBcn.vehiclePosNED = bp.toftype();
    rngBcn.vehiclePosErr = bperr;

    // Check if the range beacon data can be used to align the vehicle position
    if ((imuSampleTime_ms - rngBcn.last3DmeasTime_ms < 250) && (rngBcn.vehiclePosErr < 1.0f) && rngBcn.alignmentCompleted) {
        // check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
        const ftype posDiffSq = sq(rngBcn.receiverPos.x - rngBcn.vehiclePosNED.x) + sq(rngBcn.receiverPos.y - rngBcn.vehiclePosNED.y);
        const ftype posDiffVar = sq(rngBcn.vehiclePosErr) + rngBcn.receiverPosCov[0][0] + rngBcn.receiverPosCov[1][1];
        if (posDiffSq < 9.0f * posDiffVar) {
            rngBcn.goodToAlign = true;
            // Set the EKF origin and magnetic field declination if not previously set
            if (!validOrigin && (PV_AidingMode != AID_ABSOLUTE)) {
                // get origin from beacon system
                Location origin_loc;
                if (beacon->get_origin(origin_loc)) {
                    setOriginLLH(origin_loc);

                    // set the NE earth magnetic field states using the published declination
                    // and set the corresponding variances and covariances
                    alignMagStateDeclination();

                    // Set the uncertainty of the origin height
                    ekfOriginHgtVar = sq(rngBcn.vehiclePosErr);
                }
            }
        } else {
            rngBcn.goodToAlign = false;
        }
    } else {
        rngBcn.goodToAlign = false;
    }

    // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
    rngBcn.dataToFuse = rngBcn.storedRange.recall(rngBcn.dataDelayed, imuDataDelayed.time_ms);

    // Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
    if (rngBcn.dataToFuse) {
        rngBcn.dataDelayed.beacon_posNED.x += rngBcn.posOffsetNED.x;
        rngBcn.dataDelayed.beacon_posNED.y += rngBcn.posOffsetNED.y;
    }

}
#endif  // EK3_FEATURE_BEACON_FUSION

/********************************************************
*              Independant yaw sensor measurements      *
********************************************************/

void NavEKF3_core::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
{
    // 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 - yawMeasTime_ms) < frontend->sensorIntervalMin_ms) || !statesInitialised) {
        return;
    }

    yawAngDataNew.yawAng = yawAngle;
    yawAngDataNew.yawAngErr = yawAngleErr;
    if (type == 2) {
        yawAngDataNew.order = rotationOrder::TAIT_BRYAN_321;
    } else if (type == 1) {
        yawAngDataNew.order = rotationOrder::TAIT_BRYAN_312;
    } else {
        return;
    }
    yawAngDataNew.time_ms = timeStamp_ms;

    storedYawAng.push(yawAngDataNew);

    yawMeasTime_ms = timeStamp_ms;
}

// Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
void NavEKF3_core::writeDefaultAirSpeed(float airspeed, float uncertainty)
{
    defaultAirSpeed = airspeed;
    defaultAirSpeedVariance = sq(uncertainty);
}

/********************************************************
*            External Navigation Measurements           *
********************************************************/

void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
#if EK3_FEATURE_EXTERNAL_NAV
    // protect against NaN
    if (pos.is_nan() || isnan(posErr)) {
        return;
    }

    // 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) < frontend->extNavIntervalMin_ms) || !statesInitialised) {
        return;
    } else {
        extNavMeasTime_ms = timeStamp_ms;
    }

    ext_nav_elements extNavDataNew {};

    if (resetTime_ms != extNavLastPosResetTime_ms) {
        extNavDataNew.posReset = true;
        extNavLastPosResetTime_ms = resetTime_ms;
    } else {
        extNavDataNew.posReset = false;
    }

    extNavDataNew.pos = pos.toftype();
    extNavDataNew.posErr = posErr;

    // calculate timestamp
    timeStamp_ms = timeStamp_ms - delay_ms;
    // Correct for the average intersampling delay due to the filter update rate
    timeStamp_ms -= localFilterTimeStep_ms/2;
    // Prevent time delay exceeding age of oldest IMU data in the buffer
    timeStamp_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms);
    extNavDataNew.time_ms = timeStamp_ms;

    // store position data to buffer
    storedExtNav.push(extNavDataNew);

    // protect against attitude or angle being NaN
    if (!quat.is_nan() && !isnan(angErr)) {
        // extract yaw from the attitude
        ftype roll_rad, pitch_rad, yaw_rad;
        quat.to_euler(roll_rad, pitch_rad, yaw_rad);
        yaw_elements extNavYawAngDataNew;
        extNavYawAngDataNew.yawAng = yaw_rad;
        extNavYawAngDataNew.yawAngErr = MAX(angErr, radians(5.0f)); // ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
        extNavYawAngDataNew.order = rotationOrder::TAIT_BRYAN_321; // Euler rotation order is 321 (ZYX)
        extNavYawAngDataNew.time_ms = timeStamp_ms;
        storedExtNavYawAng.push(extNavYawAngDataNew);
    }
#endif // EK3_FEATURE_EXTERNAL_NAV
}

void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
#if EK3_FEATURE_EXTERNAL_NAV
    // sanity check for NaNs
    if (vel.is_nan() || isnan(err)) {
        return;
    }

    if ((timeStamp_ms - extNavVelMeasTime_ms) < frontend->extNavIntervalMin_ms) {
        return;
    }

    extNavVelMeasTime_ms = timeStamp_ms;
    useExtNavVel = true;
    // calculate timestamp
    timeStamp_ms = timeStamp_ms - delay_ms;
    // Correct for the average intersampling delay due to the filter updaterate
    timeStamp_ms -= localFilterTimeStep_ms/2;
    // Prevent time delay exceeding age of oldest IMU data in the buffer
    timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms);

    ext_nav_vel_elements extNavVelNew;
    extNavVelNew.time_ms = timeStamp_ms;
    extNavVelNew.vel = vel.toftype();
    extNavVelNew.err = err;
    extNavVelNew.corrected = false;

    storedExtNavVel.push(extNavVelNew);
#endif // EK3_FEATURE_EXTERNAL_NAV
}

/*
  update the GPS selection
 */
void NavEKF3_core::update_gps_selection(void)
{
    const auto &gps = dal.gps();

    // in normal operation use the primary GPS
    selected_gps = gps.primary_sensor();
    preferred_gps = selected_gps;

    if (frontend->_affinity & EKF_AFFINITY_GPS) {
        if (core_index < gps.num_sensors() ) {
            // always prefer our core_index, unless we don't have that
            // many GPS sensors available
            preferred_gps = core_index;
        }
        if (gps.status(preferred_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {
            // select our preferred_gps if it has a 3D fix, otherwise
            // use the primary GPS
            selected_gps = preferred_gps;
        }
    }
}

/*
  update the mag selection
 */
void NavEKF3_core::update_mag_selection(void)
{
    const auto &compass = dal.compass();

    if (frontend->_affinity & EKF_AFFINITY_MAG) {
        if (core_index < compass.get_count() &&
            compass.healthy(core_index) &&
            compass.use_for_yaw(core_index)) {
            // use core_index compass if it is healthy
            magSelectIndex = core_index;
        }
    }
}

/*
  update the baro selection
 */
void NavEKF3_core::update_baro_selection(void)
{
    auto &baro = dal.baro();

    // in normal operation use the primary baro
    selected_baro = baro.get_primary();

    if (frontend->_affinity & EKF_AFFINITY_BARO) {
        if (core_index < baro.num_instances() &&
            baro.healthy(core_index)) {
            // use core_index baro if it is healthy
            selected_baro = core_index;
        }
    }
}

/*
  update the airspeed selection
 */
void NavEKF3_core::update_airspeed_selection(void)
{
    const auto *arsp = dal.airspeed();
    if (arsp == nullptr) {
        return;
    }

    // in normal operation use the primary airspeed sensor
    selected_airspeed = arsp->get_primary();

    if (frontend->_affinity & EKF_AFFINITY_ARSP) {
        if (core_index < arsp->get_num_sensors() &&
            arsp->healthy(core_index) &&
            arsp->use(core_index)) {
            // use core_index airspeed if it is healthy
            selected_airspeed = core_index;
        }
    }
}

/*
  update sensor selections
 */
void NavEKF3_core::update_sensor_selection(void)
{
    update_gps_selection();
    update_mag_selection();
    update_baro_selection();
    update_airspeed_selection();
}

/*
  update timing statistics structure
 */
void NavEKF3_core::updateTimingStatistics(void)
{
    if (timing.count == 0) {
        timing.dtIMUavg_max = dtIMUavg;
        timing.dtIMUavg_min = dtIMUavg;
        timing.dtEKFavg_max = dtEkfAvg;
        timing.dtEKFavg_min = dtEkfAvg;
        timing.delAngDT_max = imuDataDelayed.delAngDT;
        timing.delAngDT_min = imuDataDelayed.delAngDT;
        timing.delVelDT_max = imuDataDelayed.delVelDT;
        timing.delVelDT_min = imuDataDelayed.delVelDT;
    } else {
        timing.dtIMUavg_max = MAX(timing.dtIMUavg_max, dtIMUavg);
        timing.dtIMUavg_min = MIN(timing.dtIMUavg_min, dtIMUavg);
        timing.dtEKFavg_max = MAX(timing.dtEKFavg_max, dtEkfAvg);
        timing.dtEKFavg_min = MIN(timing.dtEKFavg_min, dtEkfAvg);
        timing.delAngDT_max = MAX(timing.delAngDT_max, imuDataDelayed.delAngDT);
        timing.delAngDT_min = MIN(timing.delAngDT_min, imuDataDelayed.delAngDT);
        timing.delVelDT_max = MAX(timing.delVelDT_max, imuDataDelayed.delVelDT);
        timing.delVelDT_min = MIN(timing.delVelDT_min, imuDataDelayed.delVelDT);
    }
    timing.count++;
}

/*
  update estimates of inactive bias states. This keeps inactive IMUs
  as hot-spares so we can switch to them without causing a jump in the
  error
 */
void NavEKF3_core::learnInactiveBiases(void)
{
#if INS_MAX_INSTANCES == 1
    inactiveBias[0].gyro_bias = stateStruct.gyro_bias;
    inactiveBias[0].accel_bias = stateStruct.accel_bias;
#else
    const auto &ins = dal.ins();

    // learn gyro biases
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        if (!ins.use_gyro(i)) {
            // can't use this gyro
            continue;
        }
        if (gyro_index_active == i) {
            // use current estimates from main filter of gyro bias
            inactiveBias[i].gyro_bias = stateStruct.gyro_bias;
        } else {
            // get filtered gyro and use the difference between the
            // corrected gyro on the active IMU and the inactive IMU
            // to move the inactive bias towards the right value
            Vector3F filtered_gyro_active = ins.get_gyro(gyro_index_active).toftype() - (stateStruct.gyro_bias/dtEkfAvg);
            Vector3F filtered_gyro_inactive = ins.get_gyro(i).toftype() - (inactiveBias[i].gyro_bias/dtEkfAvg);
            Vector3F error = filtered_gyro_active - filtered_gyro_inactive;

            // prevent a single large error from contaminating bias estimate
            const ftype bias_limit = radians(5);
            error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
            error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
            error.z = constrain_ftype(error.z, -bias_limit, bias_limit);

            // slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
            // gyro bias error in around 1 minute
            inactiveBias[i].gyro_bias -= error * (1.0e-4f * dtEkfAvg);
        }
    }

    // learn accel biases
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        if (!ins.use_accel(i)) {
            // can't use this accel
            continue;
        }
        if (accel_index_active == i) {
            // use current estimates from main filter of accel bias
            inactiveBias[i].accel_bias = stateStruct.accel_bias;
        } else {
            // get filtered accel and use the difference between the
            // corrected accel on the active IMU and the inactive IMU
            // to move the inactive bias towards the right value
            Vector3F filtered_accel_active = ins.get_accel(accel_index_active).toftype() - (stateStruct.accel_bias/dtEkfAvg);
            Vector3F filtered_accel_inactive = ins.get_accel(i).toftype() - (inactiveBias[i].accel_bias/dtEkfAvg);
            Vector3F error = filtered_accel_active - filtered_accel_inactive;

            // prevent a single large error from contaminating bias estimate
            const ftype bias_limit = 1.0; // m/s/s
            error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
            error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
            error.z = constrain_ftype(error.z, -bias_limit, bias_limit);

            // slowly bring the inactive accel in line with the active
            // accel. This corrects a 0.5 m/s/s accel bias error in
            // around 1 minute
            inactiveBias[i].accel_bias -= error * (1.0e-4f * dtEkfAvg);
        }
    }
#endif
}

/*
  return declination in radians
*/
ftype NavEKF3_core::MagDeclination(void) const
{
    // if we are using the WMM tables then use the table declination
    // to ensure consistency with the table mag field. Otherwise use
    // the declination from the compass library
    if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
        return table_declination;
    }
    if (!use_compass()) {
        return 0;
    }
    return dal.compass().get_declination();
}

/*
  Update the on ground and not moving check.
  Should be called once per IMU update.
  Only updates when on ground and when operating without a magnetometer
*/
void NavEKF3_core::updateMovementCheck(void)
{
    const bool runCheck = onGround && (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
                                       yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV || yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF || !use_compass());
    if (!runCheck)
    {
        onGroundNotMoving = false;
        return;
    }

    const ftype gyro_limit = radians(3.0f);
    const ftype gyro_diff_limit = 0.2f;
    const ftype accel_limit = 1.0f;
    const ftype accel_diff_limit = 5.0f;
    const ftype hysteresis_ratio = 0.7f;
    const ftype dtEkfAvgInv = 1.0f / dtEkfAvg;

    // get latest bias corrected gyro and accelerometer data
    const auto &ins = dal.ins();
    Vector3F gyro = ins.get_gyro(gyro_index_active).toftype() - stateStruct.gyro_bias * dtEkfAvgInv;
    Vector3F accel = ins.get_accel(accel_index_active).toftype() - stateStruct.accel_bias * dtEkfAvgInv;

    if (!prevOnGround) {
        gyro_prev = gyro;
        accel_prev = accel;
        onGroundNotMoving = false;
        gyro_diff = gyro_diff_limit;
        accel_diff = accel_diff_limit;
        return;
    }

    // calculate a gyro rate of change metric
    Vector3F temp = (gyro - gyro_prev) * dtEkfAvgInv;
    gyro_prev = gyro;
    gyro_diff = 0.99f * gyro_diff + 0.01f * temp.length();

    // calculate a acceleration rate of change metric
    temp = (accel - accel_prev) * dtEkfAvgInv;
    accel_prev = accel;
    accel_diff = 0.99f * accel_diff + 0.01f * temp.length();

    const ftype gyro_length_ratio = gyro.length() / gyro_limit;
    const ftype accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit;
    const ftype gyro_diff_ratio = gyro_diff / gyro_diff_limit;
    const ftype accel_diff_ratio = accel_diff / accel_diff_limit;
    bool logStatusChange = false;
    if (onGroundNotMoving) {
        if (gyro_length_ratio > frontend->_ognmTestScaleFactor ||
            fabsF(accel_length_ratio) > frontend->_ognmTestScaleFactor ||
            gyro_diff_ratio > frontend->_ognmTestScaleFactor ||
            accel_diff_ratio > frontend->_ognmTestScaleFactor)
        {
            onGroundNotMoving = false;
            logStatusChange = true;
        }
    } else if (gyro_length_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
            fabsF(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
            gyro_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
            accel_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio)
    {
        onGroundNotMoving = true;
        logStatusChange = true;
    }

    if (logStatusChange || imuSampleTime_ms - lastMoveCheckLogTime_ms > 200) {
        lastMoveCheckLogTime_ms = imuSampleTime_ms;
#if HAL_LOGGING_ENABLED
        const struct log_XKFM pkt{
            LOG_PACKET_HEADER_INIT(LOG_XKFM_MSG),
            time_us            : dal.micros64(),
            core               : core_index,
            ongroundnotmoving  : onGroundNotMoving,
            gyro_length_ratio  : float(gyro_length_ratio),
            accel_length_ratio : float(accel_length_ratio),
            gyro_diff_ratio    : float(gyro_diff_ratio),
            accel_diff_ratio   : float(accel_diff_ratio),
        };
        AP::logger().WriteBlock(&pkt, sizeof(pkt));
#endif
    }
}

void NavEKF3_core::SampleDragData(const imu_elements &imu)
{
#if EK3_FEATURE_DRAG_FUSION
    // Average and down sample to 5Hz
    const ftype bcoef_x = frontend->_ballisticCoef_x;
    const ftype bcoef_y = frontend->_ballisticCoef_y;
    const ftype mcoef = frontend->_momentumDragCoef.get();
    const bool using_bcoef_x = bcoef_x > 1.0f;
    const bool using_bcoef_y = bcoef_y > 1.0f;
    const bool using_mcoef = mcoef > 0.001f;
    if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
        // nothing to do
        dragFusionEnabled = false;
        return;
    }

    dragFusionEnabled = true;

    // down-sample the drag specific force data by accumulating and calculating the mean when
    // sufficient samples have been collected

    dragSampleCount ++;

    // note acceleration is accumulated as a delta velocity
    dragDownSampled.accelXY.x += imu.delVel.x;
    dragDownSampled.accelXY.y += imu.delVel.y;
    dragDownSampled.time_ms += imu.time_ms;
    dragSampleTimeDelta += imu.delVelDT;

    // calculate and store means from accumulated values
    if (dragSampleTimeDelta > 0.2f - 0.5f * EKF_TARGET_DT) {
        // note conversion from accumulated delta velocity to acceleration
        dragDownSampled.accelXY.x /= dragSampleTimeDelta;
        dragDownSampled.accelXY.y /= dragSampleTimeDelta;
        dragDownSampled.time_ms /= dragSampleCount;

        // write to buffer
        storedDrag.push(dragDownSampled);

        // reset accumulators
        dragSampleCount = 0;
        dragDownSampled.accelXY.zero();
        dragDownSampled.time_ms = 0;
        dragSampleTimeDelta = 0.0f;
    }
#endif // EK3_FEATURE_DRAG_FUSION
}

/*
  get the earth mag field
 */
void NavEKF3_core::getEarthFieldTable(const Location &loc)
{
    table_earth_field_ga = AP_Declination::get_earth_field_ga(loc).toftype();
    table_declination = radians(AP_Declination::get_declination(loc.lat*1.0e-7,
                                                                loc.lng*1.0e-7));
    have_table_earth_field = true;
}

/*
  update earth field, called at 1Hz
 */
void NavEKF3_core::checkUpdateEarthField(void)
{
    if (have_table_earth_field && filterStatus.flags.using_gps) {
        Location loc = EKF_origin;
        loc.offset(stateStruct.position.x, stateStruct.position.y);
        getEarthFieldTable(loc);
    }
}