diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 8fdd1b51e5..fb1f3d25d3 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -18,7 +18,7 @@ extern const AP_HAL::HAL& hal; ********************************************************/ // Read the range finder and take new measurements if available -// Read at 20Hz and apply a median filter +// Apply a median filter void NavEKF2_core::readRangeFinder(void) { uint8_t midIndex; @@ -26,19 +26,31 @@ void NavEKF2_core::readRangeFinder(void) uint8_t minIndex; // get theoretical correct range when the vehicle is on the ground rngOnGnd = frontend->_rng.ground_clearance_cm() * 0.01f; - if (frontend->_rng.status() == RangeFinder::RangeFinder_Good && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) { - // store samples and sample time into a ring buffer - rngMeasIndex ++; - if (rngMeasIndex > 2) { - rngMeasIndex = 0; + + // read range finder at 20Hz + // TODO better way of knowing if it has new data + if ((imuSampleTime_ms - lastRngMeasTime_ms) > 50) { + + // reset the timer used to control the measurement rate + lastRngMeasTime_ms = imuSampleTime_ms; + + // store samples and sample time into a ring buffer if valid + if (frontend->_rng.status() == RangeFinder::RangeFinder_Good) { + rngMeasIndex ++; + if (rngMeasIndex > 2) { + rngMeasIndex = 0; + } + storedRngMeasTime_ms[rngMeasIndex] = imuSampleTime_ms - 25; + storedRngMeas[rngMeasIndex] = frontend->_rng.distance_cm() * 0.01f; } - storedRngMeasTime_ms[rngMeasIndex] = imuSampleTime_ms; - storedRngMeas[rngMeasIndex] = frontend->_rng.distance_cm() * 0.01f; - // check for three fresh samples and take median + + // check for three fresh samples bool sampleFresh[3]; for (uint8_t index = 0; index <= 2; index++) { sampleFresh[index] = (imuSampleTime_ms - storedRngMeasTime_ms[index]) < 500; } + + // find the median value if we have three fresh samples if (sampleFresh[0] && sampleFresh[1] && sampleFresh[2]) { if (storedRngMeas[0] > storedRngMeas[1]) { minIndex = 1; @@ -54,18 +66,20 @@ void NavEKF2_core::readRangeFinder(void) } else { midIndex = 2; } - rngMea = max(storedRngMeas[midIndex],rngOnGnd); - newDataRng = true; + rangeDataNew.time_ms = storedRngMeasTime_ms[midIndex]; + // limit the measured range to be no less than the on-ground range + rangeDataNew.rng = max(storedRngMeas[midIndex],rngOnGnd); rngValidMeaTime_ms = imuSampleTime_ms; - } else if (onGround) { - // if on ground and no return, we assume on ground range - rngMea = rngOnGnd; - newDataRng = true; + // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it + StoreRange(); + } else if (!takeOffDetected) { + // before takeoff we assume on-ground range value if there is no data + rangeDataNew.time_ms = imuSampleTime_ms; + rangeDataNew.rng = rngOnGnd; rngValidMeaTime_ms = imuSampleTime_ms; - } else { - newDataRng = false; + // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it + StoreRange(); } - lastRngMeasTime_ms = imuSampleTime_ms; } } @@ -119,7 +133,7 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa // Save data to buffer StoreOF(); // Check for data at the fusion time horizon - newDataFlow = RecallOF(); + flowDataToFuse = RecallOF(); } } @@ -243,6 +257,7 @@ void NavEKF2_core::readMagData() StoreMag(); } } + // store magnetometer data in a history array void NavEKF2_core::StoreMag() { @@ -526,6 +541,7 @@ void NavEKF2_core::readGpsData() // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin if (validOrigin) { gpsDataNew.pos = location_diff(EKF_origin, gpsloc); + gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt); StoreGPS(); // declare GPS available for use gpsNotAvailable = false; @@ -652,54 +668,26 @@ bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { * Height Measurements * ********************************************************/ -// check for new altitude measurement data and update stored measurement if available -void NavEKF2_core::readHgtData() +// check for new pressure altitude measurement data and update stored measurement if available +void NavEKF2_core::readBaroData() { // check to see if baro measurement has changed so we know if a new measurement has arrived // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer - if (frontend->_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) { - // adjust range finder measurement to allow for effect of vehicle tilt and height of sensor - baroDataNew.hgt = max(rngMea * Tnb_flow.c.z, rngOnGnd); - // calculate offset to baro data that enables baro to be used as a backup - // filter offset to reduce effect of baro noise and other transient errors on estimate - baroHgtOffset = 0.1f * (frontend->_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset; - } else if (isAiding && takeOffDetected) { - // we have lost range finder measurements and are in optical flow flight - // use baro measurement and correct for baro offset - failsafe use only as baro will drift - baroDataNew.hgt = max(frontend->_baro.get_altitude() - baroHgtOffset, rngOnGnd); - } else { - // If we are on ground and have no range finder reading, assume the nominal on-ground height - baroDataNew.hgt = rngOnGnd; - // calculate offset to baro data that enables baro to be used as a backup - // filter offset to reduce effect of baro noise and other transient errors on estimate - baroHgtOffset = 0.1f * (frontend->_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset; - } - } else { - // Normal operation is to use baro measurement - baroDataNew.hgt = frontend->_baro.get_altitude(); - } + if (frontend->_baro.get_last_update() - lastBaroReceived_ms > 70) { - // filtered baro data used to provide a reference for takeoff - // it is is reset to last height measurement on disarming in performArmingChecks() - if (!getTakeoffExpected()) { - const float gndHgtFiltTC = 0.5f; - const float dtBaro = frontend->hgtAvg_ms*1.0e-3f; - float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f); - meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha; - } else if (isAiding && getTakeoffExpected()) { - // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff - // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent + baroDataNew.hgt = frontend->_baro.get_altitude(); + + // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff + // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent + if (isAiding && getTakeoffExpected()) { baroDataNew.hgt = max(baroDataNew.hgt, meaHgtAtTakeOff); } // time stamp used to check for new measurement - lastHgtReceived_ms = frontend->_baro.get_last_update(); + lastBaroReceived_ms = frontend->_baro.get_last_update(); // estimate of time height measurement was taken, allowing for delays - baroDataNew.time_ms = lastHgtReceived_ms - frontend->_hgtDelay_ms; + 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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index a7ca02d9b4..e796bc681e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -68,26 +68,22 @@ void NavEKF2_core::SelectFlowFusion() // if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height // we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference - if ((newDataFlow || newDataRng) && tiltOK) { - // fuse range data into the terrain estimator if available - fuseRngData = newDataRng; + if ((flowDataToFuse || rangeDataToFuse) && tiltOK) { // fuse optical flow data into the terrain estimator if available and if there is no range data (range data is better) - fuseOptFlowData = (newDataFlow && !fuseRngData); + fuseOptFlowData = (flowDataToFuse && !rangeDataToFuse); // Estimate the terrain offset (runs a one state EKF) EstimateTerrainOffset(); - // Indicate we have used the range data - newDataRng = false; } // Fuse optical flow data into the main filter if not excessively tilted and we are in the correct mode - if (newDataFlow && tiltOK && PV_AidingMode == AID_RELATIVE) + if (flowDataToFuse && tiltOK && PV_AidingMode == AID_RELATIVE) { // Set the flow noise used by the fusion processes R_LOS = sq(max(frontend->_flowNoise, 0.05f)); // Fuse the optical flow X and Y axis data into the main filter sequentially FuseOptFlow(); // reset flag to indicate that no new flow data is available for fusion - newDataFlow = false; + flowDataToFuse = false; } // stop the performance timer @@ -111,7 +107,7 @@ void NavEKF2_core::EstimateTerrainOffset() float losRateSq = velHorizSq / sq(heightAboveGndEst); // don't update terrain offset state if there is no range finder and not generating enough LOS rate, or without GPS, as it is poorly observable - if (!fuseRngData && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f)) { + if (!rangeDataToFuse && (gpsNotAvailable || PV_AidingMode == AID_RELATIVE || velHorizSq < 25.0f || losRateSq < 0.01f)) { inhibitGndState = true; } else { inhibitGndState = false; @@ -132,7 +128,7 @@ void NavEKF2_core::EstimateTerrainOffset() timeAtLastAuxEKF_ms = imuSampleTime_ms; // fuse range finder data - if (fuseRngData) { + if (rangeDataToFuse) { // predict range float predRngMeas = max((terrainState - stateStruct.position[2]),rngOnGnd) / Tnb_flow.c.z; @@ -156,7 +152,7 @@ void NavEKF2_core::EstimateTerrainOffset() terrainState = max(terrainState, stateStruct.position[2] + rngOnGnd); // Calculate the measurement innovation - innovRng = predRngMeas - rngMea; + innovRng = predRngMeas - rangeDataDelayed.rng; // calculate the innovation consistency test ratio auxRngTestRatio = sq(innovRng) / (sq(frontend->_rngInnovGate) * varInnovRng); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 221278bd97..a087713c06 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -74,7 +74,7 @@ void NavEKF2_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn auxInnov = auxFlowObsInnov; HAGL = terrainState - stateStruct.position.z; rngInnov = innovRng; - range = rngMea; + range = rangeDataDelayed.rng; gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset() } @@ -87,6 +87,10 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const if (frontend->_fusionModeGPS == 3) { // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors height = max(float(frontend->_rng.max_distance_cm()) * 0.007f - 1.0f, 1.0f); + // If we are are not using the range finder as the height reference, then compensate for the difference between terrain and EKF origin + if (frontend->_altSource != 1) { + height -= terrainState; + } return true; } else { return false; @@ -443,6 +447,8 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3); bool gpsNavPossible = !gpsNotAvailable && (PV_AidingMode == AID_ABSOLUTE) && gpsGoodToAlign; bool filterHealthy = healthy() && tiltAlignComplete && yawAlignComplete; + // If GPS height useage is specified, height is considered to be inaccurate until the GPS passes all checks + bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin; // set individual flags status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) @@ -450,7 +456,7 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid - status.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid + status.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode status.flags.pred_horiz_pos_rel = (optFlowNavPossible || gpsNavPossible) && filterHealthy; // we should be able to estimate a relative position when we enter flight mode diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 4bf02e0bee..114396f76c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -85,10 +85,8 @@ void NavEKF2_core::ResetPosition(void) // reset the vertical position state using the last height measurement void NavEKF2_core::ResetHeight(void) { - // read the altimeter - readHgtData(); // write to the state vector - stateStruct.position.z = -baroDataNew.hgt; // down position from blended accel data + stateStruct.position.z = -hgtMea; terrainState = stateStruct.position.z + rngOnGnd; for (uint8_t i=0; i_fusionModeGPS <= 1) { fuseVelData = true; @@ -154,22 +153,8 @@ void NavEKF2_core::SelectVelPosFusion() fusePosData = false; } - // check for and read new height data - readHgtData(); - - // If we haven't received height data for a while, then declare the height data as being timed out - // set timeout period based on whether we have vertical GPS velocity available to constrain drift - hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms; - if (imuSampleTime_ms - lastHgtReceived_ms > hgtRetryTime_ms) { - hgtTimeout = true; - } - - // command fusion of height data - // wait until the EKF time horizon catches up with the measurement - if (RecallBaro()) { - // enable fusion - fuseHgtData = true; - } + // Select height data to be fused from the available baro, range finder and GPS sources + selectHeightForFusion(); // If we are operating without any aiding, fuse in the last known position and zero velocity // to constrain tilt drift. This assumes a non-manoeuvring vehicle @@ -242,7 +227,7 @@ void NavEKF2_core::FuseVelPosNED() observation[2] = gpsDataDelayed.vel.z; observation[3] = gpsDataDelayed.pos.x; observation[4] = gpsDataDelayed.pos.y; - observation[5] = -baroDataDelayed.hgt; + observation[5] = -hgtMea; // calculate additional error in GPS position caused by manoeuvring posErr = frontend->gpsPosVarAccScale * accNavMag; @@ -274,12 +259,7 @@ void NavEKF2_core::FuseVelPosNED() R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); R_OBS[4] = R_OBS[3]; } - R_OBS[5] = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f)); - - // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect - if (getTakeoffExpected() || getTouchdownExpected()) { - R_OBS[5] *= frontend->gndEffectBaroScaler; - } + R_OBS[5] = posDownObsNoise; // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance @@ -287,11 +267,10 @@ void NavEKF2_core::FuseVelPosNED() for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i]; - - // if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer + // if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting // the accelerometers and we should disable the GPS and barometer innovation consistency checks. - if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtReceived_ms) < (2 * frontend->hgtAvg_ms)) { + if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) { // calculate innovations for height and vertical GPS vel measurements float hgtErr = stateStruct.position.z - observation[5]; float velDErr = stateStruct.velocity.z - observation[2]; @@ -403,9 +382,8 @@ void NavEKF2_core::FuseVelPosNED() hgtTestRatio = sq(innovVelPos[5]) / (sq(frontend->_hgtInnovGate) * varInnovVelPos[5]); // fail if the ratio is > 1, but don't fail if bad IMU data hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata); - hgtTimeout = (imuSampleTime_ms - lastHgtPassTime_ms) > hgtRetryTime_ms; // Fuse height data if healthy or timed out or in constant position mode - if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE)) { + if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround)) { // Calculate a filtered value to be used by pre-flight health checks // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution if (onGround) { @@ -416,16 +394,16 @@ void NavEKF2_core::FuseVelPosNED() } else { hgtInnovFiltState = 0.0f; } - hgtHealth = true; - lastHgtPassTime_ms = imuSampleTime_ms; - // if timed out, reset the height, but do not fuse data on this time step + + // if timed out, reset the height if (hgtTimeout) { ResetHeight(); - fuseHgtData = false; + hgtTimeout = false; } - } - else { - hgtHealth = false; + + // If we have got this far then declare the height data as healthy and reset the timeout counter + hgtHealth = true; + lastHgtPassTime_ms = imuSampleTime_ms; } } @@ -461,23 +439,21 @@ void NavEKF2_core::FuseVelPosNED() else if (obsIndex == 3 || obsIndex == 4) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); - } else { + } else if (obsIndex == 5) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; - if (obsIndex == 5) { - const float gndMaxBaroErr = 4.0f; - const float gndBaroInnovFloor = -0.5f; + const float gndMaxBaroErr = 4.0f; + const float gndBaroInnovFloor = -0.5f; - if(getTouchdownExpected()) { - // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor - // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr - // this function looks like this: - // |/ - //---------|--------- - // ____/| - // / | - // / | - innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); - } + if(getTouchdownExpected()) { + // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor + // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr + // this function looks like this: + // |/ + //---------|--------- + // ____/| + // / | + // / | + innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr); } } @@ -555,4 +531,83 @@ void NavEKF2_core::FuseVelPosNED() * MISC FUNCTIONS * ********************************************************/ +// select the height measurement to be fused from the available baro, range finder and GPS sources +void NavEKF2_core::selectHeightForFusion() +{ + // Read range finder data and check for new data in the buffer + // This data is used by both height and optical flow fusion processing + readRangeFinder(); + rangeDataToFuse = RecallRange(); + + // read baro height data from the sensor and check for new data in the buffer + readBaroData(); + baroDataToFuse = RecallBaro(); + + // determine if we should be using a height source other than baro + bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500); + bool usingGpsForHgt = (frontend->_altSource == 2 && imuSampleTime_ms - lastTimeGpsReceived_ms < 500); + + // if there is new baro data to fuse, calculate filterred baro data required by other processes + if (baroDataToFuse) { + // calculate offset to baro data that enables baro to be used as a backup if we are using other height sources + if (usingRangeForHgt || usingGpsForHgt) { + calcFiltBaroOffset(); + } + // filtered baro data used to provide a reference for takeoff + // it is is reset to last height measurement on disarming in performArmingChecks() + if (!getTakeoffExpected()) { + const float gndHgtFiltTC = 0.5f; + const float dtBaro = frontend->hgtAvg_ms*1.0e-3f; + float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f); + meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha; + } + } + + // Select the height measurement source + if (rangeDataToFuse && usingRangeForHgt) { + // using range finder data + // correct for tilt using a flat earth model + if (prevTnb.c.z >= 0.7) { + hgtMea = max(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); + // enable fusion + fuseHgtData = true; + // set the observation noise + posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f)); + } else { + // disable fusion if tilted too far + fuseHgtData = false; + } + } else if (gpsDataToFuse && usingGpsForHgt) { + // using GPS data + hgtMea = gpsDataDelayed.hgt; + // enable fusion + fuseHgtData = true; + // set the observation noise to the horizontal GPS noise plus a scaler becasue GPS vertical position is usually less accurate + // TODO use VDOP/HDOP, reported accuracy or a separate parameter + posDownObsNoise = sq(constrain_float(frontend->_gpsHorizPosNoise * 1.5f, 0.1f, 10.0f)); + } else if (baroDataToFuse && !usingRangeForHgt && !usingGpsForHgt) { + // using Baro data + hgtMea = baroDataDelayed.hgt - baroHgtOffset; + // enable fusion + fuseHgtData = true; + // set the observation noise + posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f)); + // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect + if (getTakeoffExpected() || getTouchdownExpected()) { + posDownObsNoise *= frontend->gndEffectBaroScaler; + } + } else { + fuseHgtData = false; + } + + // If we haven't fused height data for a while, then declare the height data as being timed out + // set timeout period based on whether we have vertical GPS velocity available to constrain drift + hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms; + if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) { + hgtTimeout = true; + } else { + hgtTimeout = false; + } +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 1da0036d88..ca38c56be9 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -298,7 +298,7 @@ void NavEKF2_core::detectFlight() } // trigger if more than 10m away from initial height - if (fabsf(baroDataDelayed.hgt) > 10.0f) { + if (fabsf(hgtMea) > 10.0f) { largeHgtChange = true; } @@ -337,7 +337,7 @@ void NavEKF2_core::detectFlight() } // If rangefinder has increased since exiting on-ground, then we definitely are flying - if (!onGround && ((rngMea - rngAtStartOfFlight) > 0.5f)) { + if (!onGround && ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f)) { inFlight = true; } @@ -352,7 +352,7 @@ void NavEKF2_core::detectFlight() // store vertical position at start of flight to use as a reference for ground relative checks posDownAtTakeoff = stateStruct.position.z; // store the range finder measurement which will be used as a reference to detect when we have taken off - rngAtStartOfFlight = rngMea; + rngAtStartOfFlight = rangeDataNew.rng; } } @@ -411,7 +411,7 @@ void NavEKF2_core::detectOptFlowTakeoff(void) angRateVec = ins.get_gyro() - gyroBias; } - takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rngMea > (rngAtStartOfFlight + 0.1f))); + takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f))); } else if (onGround) { // we are confidently on the ground so set the takeoff detected status to false takeOffDetected = false; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 0f3e09dfb1..b13cd502e0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -103,7 +103,7 @@ void NavEKF2_core::InitialiseVariables() prevTasStep_ms = imuSampleTime_ms; prevBetaStep_ms = imuSampleTime_ms; lastMagUpdate_us = 0; - lastHgtReceived_ms = imuSampleTime_ms; + lastBaroReceived_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms; lastPosPassTime_ms = imuSampleTime_ms; lastHgtPassTime_ms = imuSampleTime_ms; @@ -145,13 +145,12 @@ void NavEKF2_core::InitialiseVariables() memset(&nextP[0][0], 0, sizeof(nextP)); memset(&processNoise[0], 0, sizeof(processNoise)); flowDataValid = false; - newDataRng = false; + rangeDataToFuse = false; fuseOptFlowData = false; Popt = 0.0f; terrainState = 0.0f; prevPosN = stateStruct.position.x; prevPosE = stateStruct.position.y; - fuseRngData = false; inhibitGndState = true; flowGyroBias.x = 0; flowGyroBias.y = 0; @@ -187,6 +186,7 @@ void NavEKF2_core::InitialiseVariables() yawAlignComplete = false; stateIndexLim = 23; baroStoreIndex = 0; + rangeStoreIndex = 0; magStoreIndex = 0; gpsStoreIndex = 0; tasStoreIndex = 0; @@ -297,7 +297,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) ResetPosition(); // read the barometer and set the height state - readHgtData(); + readBaroData(); ResetHeight(); // define Earth rotation vector in the NED navigation frame @@ -409,9 +409,6 @@ void NavEKF2_core::UpdateFilter(bool predict) // Predict the covariance growth CovariancePrediction(); - // Read range finder data which is used by both position and optical flow fusion - readRangeFinder(); - // Update states using magnetometer data SelectMagFusion(); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index bd90a061d3..3b895ddaba 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -505,7 +505,7 @@ private: void readGpsData(); // check for new altitude measurement data and update stored measurement if available - void readHgtData(); + void readBaroData(); // check for new magnetometer data and update store measurements if available void readMagData(); @@ -632,6 +632,9 @@ private: // calculate a filtered offset between baro height measurement and EKF height estimate void calcFiltBaroOffset(); + // Select height data to be fused from the available baro, range finder and GPS sources + void selectHeightForFusion(); + // Length of FIFO buffers used for non-IMU sensor data. // Must be larger than the time period defined by IMU_BUFFER_LENGTH static const uint32_t OBS_BUFFER_LENGTH = 5; @@ -699,7 +702,7 @@ private: uint32_t imuSampleTime_ms; // time that the last IMU value was taken bool newDataTas; // true when new airspeed data has arrived bool tasDataWaiting; // true when new airspeed data is waiting to be fused - uint32_t lastHgtReceived_ms; // time last time we received height data + uint32_t lastBaroReceived_ms; // time last time we received baro height data uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec) uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec) @@ -796,6 +799,7 @@ private: uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycele uint8_t localFilterTimeStep_ms; // average number of msec between filter updates + float posDownObsNoise; // observationn noise on the vertical position used by the state and covariance update step (m) // variables used to calulate a vertical velocity that is kinematically consistent with the verical position float posDownDerivative; // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD. @@ -827,7 +831,7 @@ private: of_elements ofDataNew; // OF data at the current time horizon of_elements ofDataDelayed; // OF data at the fusion time horizon uint8_t ofStoreIndex; // OF data storage index - bool newDataFlow; // true when new optical flow data has arrived + bool flowDataToFuse; // true when optical flow data has is ready for fusion bool flowDataValid; // true while optical flow data is still fresh bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused float auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator @@ -847,10 +851,9 @@ private: float terrainState; // terrain position state (m) float prevPosN; // north position at last measurement float prevPosE; // east position at last measurement - bool fuseRngData; // true when fusion of range data is demanded float varInnovRng; // range finder observation innovation variance (m^2) float innovRng; // range finder observation innovation (m) - float rngMea; // range finder measurement (m) + float hgtMea; // height measurement derived from either baro, gps or range finder data (m) bool inhibitGndState; // true when the terrain position state is to remain constant uint32_t prevFlowFuseTime_ms; // time both flow measurement components passed their innovation consistency checks Vector2 flowTestRatio; // square of optical flow innovations divided by fail threshold used by main filter where >1.0 is a fail @@ -858,7 +861,9 @@ private: float R_LOS; // variance of optical flow rate measurements (rad/sec)^2 float auxRngTestRatio; // square of range finder innovations divided by fail threshold used by main filter where >1.0 is a fail Vector2f flowGyroBias; // bias error of optical flow sensor gyro output - bool newDataRng; // true when new valid range finder data has arrived. + bool rangeDataToFuse; // true when valid range finder height data has arrived at the fusion time horizon. + bool baroDataToFuse; // true when valid baro height finder data has arrived at the fusion time horizon. + bool gpsDataToFuse; // true when valid GPS data has arrived at the fusion time horizon. Vector2f heldVelNE; // velocity held when no aiding is available enum AidingMode {AID_ABSOLUTE=0, // GPS aiding is being used (optical flow may also be used) so position estimates are absolute. AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.