From c5e465aec9ca02be1efa2fd943a930ccd4266e5f Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 15 Apr 2020 09:56:28 +0900 Subject: [PATCH] AP_NavEKF3: add external nav system support includes decoupling height source from use of external nav data --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 24 ++++- libraries/AP_NavEKF3/AP_NavEKF3.h | 15 ++- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 28 +++++- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 45 +++++++++ libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 96 +++++++++++++------ libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 15 +++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 49 ++++++++-- 9 files changed, 232 insertions(+), 44 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 686ac103b4..0bfd9f62ac 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -196,8 +196,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Param: ALT_SOURCE // @DisplayName: Primary altitude sensor source - // @Description: Primary height sensor used by the EKF. If the selected option cannot be used, baro is used. 1 uses the range finder and only with optical flow navigation (EK2_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground. - // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon + // @Description: Primary height sensor used by the EKF. If the selected option cannot be used, baro is used. 1 uses the range finder and only with optical flow navigation (EK3_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground. Setting 4 uses external nav system data, but only if data is also being used for horizontal position + // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon, 4:Use External Nav // @User: Advanced // @RebootRequired: True AP_GROUPINFO("ALT_SOURCE", 9, NavEKF3, _altSource, 0), @@ -1315,6 +1315,26 @@ void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t tim } } +/* + * Write position and quaternion data from an external navigation system + * + * pos : XYZ position (m) in a RH navigation frame with the Z axis pointing down and XY axes horizontal. Frame must be aligned with NED if the magnetomer is being used for yaw. + * quat : quaternion describing the the rotation from navigation frame to body frame + * posErr : 1-sigma spherical position error (m) + * angErr : 1-sigma spherical angle error (rad) + * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec) + * resetTime_ms : system time of the last position reset request (mSec) + * +*/ +void NavEKF3::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) +{ + if (core) { + for (uint8_t i=0; i_flowUse == FLOW_USE_NAV)) || readyToUseBodyOdm()) { PV_AidingMode = AID_RELATIVE; @@ -251,6 +251,9 @@ void NavEKF3_core::setAidingMode() bool gpsPosUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); + // Check if external nav position is being used + bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms); + // Check if attitude drift has been constrained by a measurement source bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed; @@ -258,7 +261,7 @@ void NavEKF3_core::setAidingMode() bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed; // check if position drift has been constrained by a measurement source - bool posAiding = gpsPosUsed || rngBcnUsed; + bool posAiding = gpsPosUsed || rngBcnUsed || extNavUsed; // Check if the loss of attitude aiding has become critical bool attAidLossCritical = false; @@ -294,12 +297,14 @@ void NavEKF3_core::setAidingMode() rngBcnTimeout = true; tasTimeout = true; gpsNotAvailable = true; + extNavTimeout = true; } else if (posAidLossCritical) { // if the loss of position is critical, declare all sources of position aiding as being timed out posTimeout = true; velTimeout = true; rngBcnTimeout = true; gpsNotAvailable = true; + extNavTimeout = true; } else if (posAidLossPending) { // attempt to reset the yaw to the estimate from the EKF-GSF algorithm @@ -364,6 +369,16 @@ void NavEKF3_core::setAidingMode() gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index); gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y); gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffsetNED.z); + } else if (readyToUseExtNav()) { + // we are commencing aiding using external nav + posResetSource = EXTNAV; + velResetSource = DEFAULT; + gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using external nav data",(unsigned)imu_index); + gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z); + // handle height reset as special case + hgtMea = -extNavDataDelayed.pos.z; + posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); + ResetHeight(); } // clear timeout flags as a precaution to avoid triggering any additional transitions @@ -456,6 +471,12 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcnAlignmentCompleted && rngBcnDataToFuse; } +// return true if the filter is ready to use external nav data +bool NavEKF3_core::readyToUseExtNav(void) const +{ + return tiltAlignComplete && extNavDataToFuse; +} + // return true if we should use the compass bool NavEKF3_core::use_compass(void) const { @@ -489,7 +510,8 @@ bool NavEKF3_core::assume_zero_sideslip(void) const // set the LLH location of the filters NED origin bool NavEKF3_core::setOriginLLH(const Location &loc) { - if (PV_AidingMode == AID_ABSOLUTE) { + if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3)) { + // reject attempt to set origin if GPS is being used return false; } EKF_origin = loc; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index f027543be2..1ffaa91e1f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -920,7 +920,7 @@ void NavEKF3_core::FuseMagnetometer() * usePredictedYaw - Set this to true if no valid yaw measurement will be available for an extended periods. * This uses an innovation set to zero which prevents uncontrolled quaternion covariance * growth or if available, a yaw estimate from the Gaussian Sum Filter. - * UseExternalYawSensor - Set this to true if yaw data from an external yaw sensor is being used instead of the magnetometer. + * UseExternalYawSensor - Set this to true if yaw data from an external yaw sensor (GPS or external nav) is being used instead of the magnetometer. */ void NavEKF3_core::fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 4f94adfbeb..e469296e8a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -936,6 +936,51 @@ void NavEKF3_core::writeDefaultAirSpeed(float airspeed) defaultAirSpeed = airspeed; } +/******************************************************** +* External Navigation Measurements * +********************************************************/ + +void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms) +{ + // 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) < 70) || !statesInitialised) { + return; + } else { + extNavMeasTime_ms = timeStamp_ms; + } + + if (resetTime_ms != extNavLastPosResetTime_ms) { + extNavDataNew.posReset = true; + extNavLastPosResetTime_ms = resetTime_ms; + } else { + extNavDataNew.posReset = false; + } + + extNavDataNew.pos = pos; + if (posErr > 0) { + extNavDataNew.posErr = posErr; + } else { + extNavDataNew.posErr = frontend->_gpsHorizPosNoise; + } + + // calculate timestamp + const uint32_t extnav_delay_ms = 10; + timeStamp_ms = timeStamp_ms - extnav_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; + + // extract yaw from the attitude + float roll_rad, pitch_rad, yaw_rad; + quat.to_euler(roll_rad, pitch_rad, yaw_rad); + writeEulerYawAngle(yaw_rad, angErr, timeStamp_ms, 2); + + storedExtNav.push(extNavDataNew); +} + /* update timing statistics structure */ diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index a0604b9d5c..5a41c2dbaa 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -111,7 +111,7 @@ bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, fl bool NavEKF3_core::getHeightControlLimit(float &height) const { // only ask for limiting if we are doing optical flow navigation - if (frontend->_fusionModeGPS == 3) { + if (frontend->_fusionModeGPS == 3 && (PV_AidingMode == AID_RELATIVE) && flowDataValid) { // If are doing optical flow nav, ensure the height above ground is within range finder limits after accounting for vehicle tilt and control errors const RangeFinder *_rng = AP::rangefinder(); if (_rng == nullptr) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 5bd3897a9d..88e2b22de1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -119,6 +119,15 @@ void NavEKF3_core::ResetPosition(void) // clear the timeout flags and counters rngBcnTimeout = false; lastRngBcnPassTime_ms = imuSampleTime_ms; + } else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == DEFAULT) || posResetSource == EXTNAV) { + // use external nav system data as the third preference + stateStruct.position.x = extNavDataDelayed.pos.x; + stateStruct.position.y = extNavDataDelayed.pos.y; + // set the variances as received from external nav system data + P[7][7] = P[8][8] = sq(extNavDataDelayed.posErr); + // clear the timeout flags and counters + extNavTimeout = false; + lastExtNavPassTime_ms = imuSampleTime_ms; } } for (uint8_t i=0; i_fusionModeGPS != 3)) { // Don't fuse velocity data if GPS doesn't support it if (frontend->_fusionModeGPS <= 1) { @@ -300,8 +312,28 @@ void NavEKF3_core::SelectVelPosFusion() fuseVelData = false; } fusePosData = true; + extNavUsedForPos = false; CorrectGPSForAntennaOffset(gpsDataDelayed); + + // copy corrected GPS data to observation vector + if (fuseVelData) { + velPosObs[0] = gpsDataDelayed.vel.x; + velPosObs[1] = gpsDataDelayed.vel.y; + velPosObs[2] = gpsDataDelayed.vel.z; + } + velPosObs[3] = gpsDataDelayed.pos.x; + velPosObs[4] = gpsDataDelayed.pos.y; + } else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS == 3)) { + // use external nav system for position + extNavUsedForPos = true; + activeHgtSource = HGT_SOURCE_EXTNAV; + fuseVelData = false; + fuseHgtData = true; + fusePosData = true; + velPosObs[3] = extNavDataDelayed.pos.x; + velPosObs[4] = extNavDataDelayed.pos.y; + velPosObs[5] = extNavDataDelayed.pos.z; } else { fuseVelData = false; fusePosData = false; @@ -369,13 +401,14 @@ void NavEKF3_core::SelectVelPosFusion() } } + // To-Do: add external nav position reset handling here? + // If we are operating without any aiding, fuse in the last known position // to constrain tilt drift. This assumes a non-manoeuvring vehicle // Do this to coincide with the height fusion if (fuseHgtData && PV_AidingMode == AID_NONE) { - gpsDataDelayed.vel.zero(); - gpsDataDelayed.pos.x = lastKnownPositionNE.x; - gpsDataDelayed.pos.y = lastKnownPositionNE.y; + velPosObs[3] = lastKnownPositionNE.x; + velPosObs[4] = lastKnownPositionNE.y; fusePosData = true; fuseVelData = false; @@ -413,7 +446,6 @@ void NavEKF3_core::FuseVelPosNED() // declare variables used by state and covariance update calculations Vector6 R_OBS; // Measurement variances used for fusion Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only - Vector6 observation; float SK; // perform sequential fusion of GPS measurements. This assumes that the @@ -423,17 +455,11 @@ void NavEKF3_core::FuseVelPosNED() // so we might as well take advantage of the computational efficiencies // associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { - // form the observation vector - observation[0] = gpsDataDelayed.vel.x; - observation[1] = gpsDataDelayed.vel.y; - observation[2] = gpsDataDelayed.vel.z; - observation[3] = gpsDataDelayed.pos.x; - observation[4] = gpsDataDelayed.pos.y; - observation[5] = -hgtMea; - // calculate additional error in GPS position caused by manoeuvring float posErr = frontend->gpsPosVarAccScale * accNavMag; + // To-Do: this posErr should come from external nav when fusing external nav position + // estimate the GPS Velocity, GPS horiz position and height measurement variances. // Use different errors if operating without external aiding using an assumed position or velocity of zero if (PV_AidingMode == AID_NONE) { @@ -463,6 +489,8 @@ void NavEKF3_core::FuseVelPosNED() // Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter if (gpsPosAccuracy > 0.0f) { R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f)); + } else if (extNavUsedForPos) { + R_OBS[3] = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f)); } else { R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); } @@ -480,8 +508,8 @@ void NavEKF3_core::FuseVelPosNED() // the accelerometers and we should disable the GPS and barometer innovation consistency checks. 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]; + const float hgtErr = stateStruct.position.z - velPosObs[5]; + const float velDErr = stateStruct.velocity.z - velPosObs[2]; // check if they are the same sign and both more than 3-sigma out of bounds if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[9][9] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[6][6] + R_OBS_DATA_CHECKS[2]))) { badIMUdata = true; @@ -494,8 +522,8 @@ void NavEKF3_core::FuseVelPosNED() // test position measurements if (fusePosData) { // test horizontal position measurements - innovVelPos[3] = stateStruct.position.x - observation[3]; - innovVelPos[4] = stateStruct.position.y - observation[4]; + innovVelPos[3] = stateStruct.position.x - velPosObs[3]; + innovVelPos[4] = stateStruct.position.y - velPosObs[4]; varInnovVelPos[3] = P[7][7] + R_OBS_DATA_CHECKS[3]; varInnovVelPos[4] = P[8][8] + R_OBS_DATA_CHECKS[4]; // apply an innovation consistency threshold test, but don't fail if bad IMU data @@ -546,7 +574,7 @@ void NavEKF3_core::FuseVelPosNED() // velocity states start at index 4 stateIndex = i + 4; // calculate innovations using blended and single IMU predicted states - velInnov[i] = stateStruct.velocity[i] - observation[i]; // blended + velInnov[i] = stateStruct.velocity[i] - velPosObs[i]; // blended // calculate innovation variance varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i]; // sum the innovation and innovation variances @@ -580,7 +608,7 @@ void NavEKF3_core::FuseVelPosNED() // test height measurements if (fuseHgtData) { // calculate height innovations - innovVelPos[5] = stateStruct.position.z - observation[5]; + innovVelPos[5] = stateStruct.position.z - velPosObs[5]; varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5]; // calculate the innovation consistency test ratio hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]); @@ -639,16 +667,14 @@ void NavEKF3_core::FuseVelPosNED() stateIndex = 4 + obsIndex; // calculate the measurement innovation, using states from a different time coordinate if fusing height data // adjust scaling on GPS measurement noise variances if not enough satellites - if (obsIndex <= 2) - { - innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - observation[obsIndex]; + if (obsIndex <= 2) { + innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - velPosObs[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); - } - else if (obsIndex == 3 || obsIndex == 4) { - innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; + } else if (obsIndex == 3 || obsIndex == 4) { + innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex]; R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 5) { - innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - observation[obsIndex]; + innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex]; const float gndMaxBaroErr = 4.0f; const float gndBaroInnovFloor = -0.5f; @@ -814,7 +840,10 @@ void NavEKF3_core::selectHeightForFusion() baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms); // select height source - if (_rng && ((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { + if (extNavUsedForPos && (frontend->_altSource == 4)) { + // always use external navigation as the height source if using for position. + activeHgtSource = HGT_SOURCE_EXTNAV; + } else if (_rng && ((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { if (frontend->_altSource == 1) { // always use range finder activeHgtSource = HGT_SOURCE_RNG; @@ -865,10 +894,11 @@ void NavEKF3_core::selectHeightForFusion() activeHgtSource = HGT_SOURCE_BARO; } - // Use Baro alt as a fallback if we lose range finder or GPS + // Use Baro alt as a fallback if we lose range finder, GPS or external nav bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500)); bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); - if (lostRngHgt || lostGpsHgt) { + bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000)); + if (lostRngHgt || lostGpsHgt || lostExtNavHgt) { activeHgtSource = HGT_SOURCE_BARO; } @@ -899,7 +929,10 @@ void NavEKF3_core::selectHeightForFusion() } // Select the height measurement source - if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) { + if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) { + hgtMea = -extNavDataDelayed.pos.z; + posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); + } else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) { // using range finder data // correct for tilt using a flat earth model if (prevTnb.c.z >= 0.7) { @@ -907,6 +940,7 @@ void NavEKF3_core::selectHeightForFusion() hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); // correct for terrain position relative to datum hgtMea -= terrainState; + velPosObs[5] = -hgtMea; // enable fusion fuseHgtData = true; // set the observation noise @@ -920,6 +954,7 @@ void NavEKF3_core::selectHeightForFusion() } else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) { // using GPS data hgtMea = gpsDataDelayed.hgt; + velPosObs[5] = -hgtMea; // enable fusion fuseHgtData = true; // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio @@ -944,6 +979,7 @@ void NavEKF3_core::selectHeightForFusion() if (motorsArmed && getTakeoffExpected()) { hgtMea = MAX(hgtMea, meaHgtAtTakeOff); } + velPosObs[5] = -hgtMea; } else { fuseHgtData = false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 98d87ebbf4..a8d8469944 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -140,6 +140,9 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) if(!storedRangeBeacon.init(imu_buffer_length)) { return false; } + if (!storedExtNav.init(obs_buffer_length)) { + return false; + } if(!storedIMU.init(imu_buffer_length)) { return false; } @@ -329,6 +332,7 @@ void NavEKF3_core::InitialiseVariables() ekfGpsRefHgt = 0.0; velOffsetNED.zero(); posOffsetNED.zero(); + memset(&velPosObs, 0, sizeof(velPosObs)); posResetSource = DEFAULT; velResetSource = DEFAULT; @@ -390,6 +394,16 @@ void NavEKF3_core::InitialiseVariables() memset(&yawAngDataNew, 0, sizeof(yawAngDataNew)); memset(&yawAngDataDelayed, 0, sizeof(yawAngDataDelayed)); + // external nav data fusion + memset(&extNavDataNew, 0, sizeof(extNavDataNew)); + memset(&extNavDataDelayed, 0, sizeof(extNavDataDelayed)); + extNavMeasTime_ms = 0; + extNavLastPosResetTime_ms = 0; + lastExtNavPassTime_ms = 0; + extNavDataToFuse = false; + extNavUsedForPos = false; + extNavTimeout = false; + // zero data buffers storedIMU.reset(); storedGPS.reset(); @@ -400,6 +414,7 @@ void NavEKF3_core::InitialiseVariables() storedRangeBeacon.reset(); storedBodyOdm.reset(); storedWheelOdm.reset(); + storedExtNav.reset(); // initialise pre-arm message hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising"); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 070db4bd6f..ad9035c68b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -43,10 +43,11 @@ #define MASK_GPS_HORIZ_SPD (1<<7) // active height source -#define HGT_SOURCE_BARO 0 -#define HGT_SOURCE_RNG 1 -#define HGT_SOURCE_GPS 2 -#define HGT_SOURCE_BCN 3 +#define HGT_SOURCE_BARO 0 +#define HGT_SOURCE_RNG 1 +#define HGT_SOURCE_GPS 2 +#define HGT_SOURCE_BCN 3 +#define HGT_SOURCE_EXTNAV 4 #define earthRate 0.000072921f // earth rotation rate (rad/sec) @@ -286,6 +287,19 @@ public: */ void writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type); + /* + * Write position and quaternion data from an external navigation system + * + * pos : position in the RH navigation frame. Frame is assumed to be NED if frameIsNED is true. (m) + * quat : quaternion desribing the the rotation from navigation frame to body frame + * posErr : 1-sigma spherical position error (m) + * angErr : 1-sigma spherical angle error (rad) + * timeStamp_ms : system time the measurement was taken, not the time it was received (mSec) + * resetTime_ms : system time of the last position reset request (mSec) + * + */ + void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint32_t resetTime_ms); + // called by vehicle code to specify that a takeoff is happening // causes the EKF to compensate for expected barometer errors due to ground effect void setTakeoffExpected(bool val); @@ -572,6 +586,13 @@ private: uint8_t type; // type specifiying Euler rotation order used, 1 = 312 (ZXY), 2 = 321 (ZYX) }; + struct ext_nav_elements { + Vector3f pos; // XYZ position measured in a RH navigation frame (m) + float posErr; // spherical poition measurement error 1-std (m) + uint32_t time_ms; // measurement timestamp (msec) + bool posReset; // true when the position measurement has been reset + }; + // bias estimates for the IMUs that are enabled but not being used // by this core. struct { @@ -777,6 +798,9 @@ private: // return true if the filter is ready to start using body frame odometry measurements bool readyToUseBodyOdm(void) const; + // return true if the filter to be ready to use external nav data + bool readyToUseExtNav(void) const; + // return true if we should use the range finder sensor bool useRngFinder(void) const; @@ -949,6 +973,7 @@ private: uint32_t airborneDetectTime_ms; // last time flight movement was detected Vector6 innovVelPos; // innovation output for a group of measurements Vector6 varInnovVelPos; // innovation variance output for a group of measurements + Vector6 velPosObs; // observations for combined velocity and positon group of measurements (3x1 m , 3x1 m/s) bool fuseVelData; // this boolean causes the velNED measurements to be fused bool fusePosData; // this boolean causes the posNE measurements to be fused bool fuseHgtData; // this boolean causes the hgtMea measurements to be fused @@ -978,7 +1003,7 @@ private: uint32_t secondLastGpsTime_ms; // time of second last GPS fix used to determine how long since last update uint32_t lastHealthyMagTime_ms; // time the magnetometer was last declared healthy bool allMagSensorsFailed; // true if all magnetometer sensors have timed out on this flight and we are no longer using magnetometer data - uint32_t lastSynthYawTime_ms; // time stamp when synthetic yaw measurement was last fused to maintain covariance health (msec) + uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec) uint32_t ekfStartTime_ms; // time the EKF was started (msec) Vector2f lastKnownPositionNE; // last known position uint32_t lastDecayTime_ms; // time of last decay of GPS position offset @@ -1089,7 +1114,8 @@ private: FLOW=3, // Use optical flow rates BARO=4, // Use Baro height MAG=5, // Use magnetometer data - RNGFND=6 // Use rangefinder data + RNGFND=6, // Use rangefinder data + EXTNAV=7 // Use external nav data }; resetDataSource posResetSource; // preferred source of data for position reset resetDataSource velResetSource; // preferred source of data for a velocity reset @@ -1292,6 +1318,17 @@ private: bool onGroundNotMoving; // true when on the ground and not moving uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec) + // external navigation fusion + obs_ring_buffer_t storedExtNav; // external navigation data buffer + ext_nav_elements extNavDataNew; // External nav data at the current time horizon + ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon + uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec) + uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec) + uint32_t lastExtNavPassTime_ms; // time stamp when external nav position measurement last passed innovation consistency check (msec) + bool extNavDataToFuse; // true when there is new external nav data to fuse + bool extNavUsedForPos; // true when the external nav data is being used as a position reference. + bool extNavTimeout; // true if external nav measurements have failed innovation consistency checks for too long + // flags indicating severe numerical errors in innovation variance calculation for different fusion operations struct { bool bad_xmag:1;