From 1ccda938cb178d7ab239a4bdff7b121b3642185f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Jan 2021 15:27:03 +1100 Subject: [PATCH] AP_NavEKF3: make external navigation optional --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 8 ++++ libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 8 ++++ .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 ++ libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 2 + .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 48 ++++++++++++++++--- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 8 ++++ libraries/AP_NavEKF3/AP_NavEKF3_core.h | 4 +- libraries/AP_NavEKF3/AP_NavEKF3_feature.h | 8 ++++ 8 files changed, 83 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 9bd1a2e48b..30f311d31c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -373,6 +373,7 @@ 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); +#if EK3_FEATURE_EXTERNAL_NAV } else if (readyToUseExtNav()) { // we are commencing aiding using external nav posResetSource = resetDataSource::EXTNAV; @@ -386,6 +387,7 @@ void NavEKF3_core::setAidingMode() hgtMea = -extNavDataDelayed.pos.z; posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); ResetHeight(); +#endif // EK3_FEATURE_EXTERNAL_NAV } // clear timeout flags as a precaution to avoid triggering any additional transitions @@ -505,11 +507,15 @@ bool NavEKF3_core::readyToUseRangeBeacon(void) const // return true if the filter is ready to use external nav data bool NavEKF3_core::readyToUseExtNav(void) const { +#if EK3_FEATURE_EXTERNAL_NAV if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) { return false; } return tiltAlignComplete && extNavDataToFuse; +#else + return false; +#endif // EK3_FEATURE_EXTERNAL_NAV } // return true if we should use the compass @@ -531,6 +537,7 @@ bool NavEKF3_core::use_compass(void) const // are we using a yaw source other than the magnetomer? bool NavEKF3_core::using_external_yaw(void) const { +#if EK3_FEATURE_EXTERNAL_NAV const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS || yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK || yaw_source == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) { @@ -539,6 +546,7 @@ bool NavEKF3_core::using_external_yaw(void) const if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) { return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000)); } +#endif return false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 17e4816a35..18d3f7bb44 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -326,6 +326,7 @@ void NavEKF3_core::SelectMagFusion() // fall through to magnetometer fusion } +#if EK3_FEATURE_EXTERNAL_NAV // Handle case where we are using an external nav for yaw const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms); if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTNAV) { @@ -353,6 +354,7 @@ void NavEKF3_core::SelectMagFusion() } } } +#endif // EK3_FEATURE_EXTERNAL_NAV // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout if (magHealth) { @@ -905,9 +907,11 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) R_YAW = sq(frontend->_yawNoise); break; +#if EK3_FEATURE_EXTERNAL_NAV case yawFusionMethod::EXTNAV: R_YAW = sq(MAX(extNavYawAngDataDelayed.yawAngErr, 0.05f)); break; +#endif } // determine if a 321 or 312 Euler sequence is best @@ -929,9 +933,11 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) order = (fabsf(prevTnb[0][2]) < fabsf(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312; break; +#if EK3_FEATURE_EXTERNAL_NAV case yawFusionMethod::EXTNAV: order = extNavYawAngDataDelayed.order; break; +#endif } // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix @@ -1091,9 +1097,11 @@ bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method) innovYaw = 0.0f; break; +#if EK3_FEATURE_EXTERNAL_NAV case yawFusionMethod::EXTNAV: innovYaw = wrap_PI(yawAngPredicted - extNavYawAngDataDelayed.yawAng); break; +#endif } // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 834e5a2d1e..f91e6cb0fd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -990,6 +990,7 @@ void NavEKF3_core::writeDefaultAirSpeed(float airspeed) 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; @@ -1038,10 +1039,12 @@ void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, 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; @@ -1067,6 +1070,7 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t extNavVelNew.corrected = false; storedExtNavVel.push(extNavVelNew); +#endif // EK3_FEATURE_EXTERNAL_NAV } /* diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 900351d2e2..e0441e883b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -474,6 +474,7 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour innovations = gpsVelInnov; variances = gpsVelVarInnov; return true; +#if EK3_FEATURE_EXTERNAL_NAV case AP_NavEKF_Source::SourceXY::EXTNAV: // check for timeouts if (AP_HAL::millis() - extNavVelInnovTime_ms > 500) { @@ -482,6 +483,7 @@ bool NavEKF3_core::getVelInnovationsAndVariancesForSource(AP_NavEKF_Source::Sour innovations = extNavVelInnov; variances = extNavVelVarInnov; return true; +#endif // EK3_FEATURE_EXTERNAL_NAV default: // variances are not available for this source return false; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 78d7154389..ea6e749ff4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -35,12 +35,14 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource) stateStruct.velocity.y = gps_corrected.vel.y; // set the variances using the reported GPS speed accuracy P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy)); +#if EK3_FEATURE_EXTERNAL_NAV } else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::EXTNAV) { // use external nav data as the 2nd preference // already corrected for sensor position stateStruct.velocity.x = extNavVelDelayed.vel.x; stateStruct.velocity.y = extNavVelDelayed.vel.y; P[5][5] = P[4][4] = sq(extNavVelDelayed.err); +#endif // EK3_FEATURE_EXTERNAL_NAV } else { stateStruct.velocity.x = 0.0f; stateStruct.velocity.y = 0.0f; @@ -111,6 +113,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource) // clear the timeout flags and counters rngBcnTimeout = false; lastRngBcnPassTime_ms = imuSampleTime_ms; +#if EK3_FEATURE_EXTERNAL_NAV } else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) { // use external nav data as the third preference stateStruct.position.x = extNavDataDelayed.pos.x; @@ -120,6 +123,7 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource) // clear the timeout flags and counters posTimeout = false; lastPosPassTime_ms = imuSampleTime_ms; +#endif // EK3_FEATURE_EXTERNAL_NAV } } for (uint8_t i=0; isources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && gpsDataNew.have_vz) { stateStruct.velocity.z = gpsDataNew.vel.z; +#if EK3_FEATURE_EXTERNAL_NAV } else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) { stateStruct.velocity.z = extNavVelDelayed.vel.z; +#endif } else if (onGround) { stateStruct.velocity.z = 0.0f; } @@ -260,9 +266,12 @@ void NavEKF3_core::ResetHeight(void) zeroCols(P,6,6); // set the variances to the measurement variance +#if EK3_FEATURE_EXTERNAL_NAV if (useExtNavVel) { P[6][6] = sq(extNavVelDelayed.err); - } else { + } else +#endif + { P[6][6] = sq(frontend->_gpsVertVelNoise); } } @@ -414,6 +423,7 @@ void NavEKF3_core::SelectVelPosFusion() posVelFusionDelayed = false; } +#if EK3_FEATURE_EXTERNAL_NAV // Check for data at the fusion time horizon extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms); if (extNavDataToFuse) { @@ -429,6 +439,7 @@ void NavEKF3_core::SelectVelPosFusion() // record time innovations were calculated (for timeout checks) extNavVelInnovTime_ms = AP_HAL::millis(); } +#endif // EK3_FEATURE_EXTERNAL_NAV // Read GPS data from the sensor readGpsData(); @@ -460,8 +471,9 @@ void NavEKF3_core::SelectVelPosFusion() // Don't fuse velocity data if GPS doesn't support it fuseVelData = frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS); fusePosData = true; +#if EK3_FEATURE_EXTERNAL_NAV extNavUsedForPos = false; - +#endif // copy corrected GPS data to observation vector if (fuseVelData) { @@ -471,14 +483,17 @@ void NavEKF3_core::SelectVelPosFusion() } velPosObs[3] = gpsDataDelayed.pos.x; velPosObs[4] = gpsDataDelayed.pos.y; +#if EK3_FEATURE_EXTERNAL_NAV } else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) { // use external nav system for horizontal position extNavUsedForPos = true; fusePosData = true; velPosObs[3] = extNavDataDelayed.pos.x; velPosObs[4] = extNavDataDelayed.pos.y; +#endif // EK3_FEATURE_EXTERNAL_NAV } +#if EK3_FEATURE_EXTERNAL_NAV // fuse external navigation velocity data if available // extNavVelDelayed is already corrected for sensor position if (extNavVelToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) { @@ -487,6 +502,7 @@ void NavEKF3_core::SelectVelPosFusion() velPosObs[1] = extNavVelDelayed.vel.y; velPosObs[2] = extNavVelDelayed.vel.z; } +#endif // we have GPS data to fuse and a request to align the yaw using the GPS course if (gpsYawResetRequest) { @@ -513,6 +529,7 @@ void NavEKF3_core::SelectVelPosFusion() } } +#if EK3_FEATURE_EXTERNAL_NAV // check for external nav position reset if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || posxy_source_reset)) { // mark a source reset as consumed @@ -522,6 +539,7 @@ void NavEKF3_core::SelectVelPosFusion() ResetPositionD(-hgtMea); } } +#endif // EK3_FEATURE_EXTERNAL_NAV // If we are operating without any aiding, fuse in constant position of constant // velocity measurements to constrain tilt drift. This assumes a non-manoeuvring @@ -620,8 +638,10 @@ void NavEKF3_core::FuseVelPosNED() // use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f)); R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f)); +#if EK3_FEATURE_EXTERNAL_NAV } else if (extNavVelToFuse) { R_OBS[2] = R_OBS[0] = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f)); +#endif } else { // calculate additional error in GPS velocity caused by manoeuvring R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); @@ -631,8 +651,10 @@ 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)); +#if EK3_FEATURE_EXTERNAL_NAV } else if (extNavUsedForPos) { R_OBS[3] = sq(constrain_float(extNavDataDelayed.posErr, 0.01f, 10.0f)); +#endif } else { R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr); } @@ -641,9 +663,12 @@ void NavEKF3_core::FuseVelPosNED() // 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 performance // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early float obs_data_chk; +#if EK3_FEATURE_EXTERNAL_NAV if (extNavVelToFuse) { obs_data_chk = sq(constrain_float(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag); - } else { + } else +#endif + { obs_data_chk = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag); } R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk; @@ -984,7 +1009,9 @@ void NavEKF3_core::selectHeightForFusion() baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms); bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500); +#if EK3_FEATURE_EXTERNAL_NAV const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500); +#endif // select height source if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) { // user has specified the range finder as a primary height source @@ -1033,16 +1060,22 @@ void NavEKF3_core::selectHeightForFusion() activeHgtSource = AP_NavEKF_Source::SourceZ::GPS; } else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcnGoodToAlign) { activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON; +#if EK3_FEATURE_EXTERNAL_NAV } else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV) && extNavDataIsFresh) { activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV; +#endif } // Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh); bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); - bool lostExtNavHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) && !extNavDataIsFresh); bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000)); - if (lostRngHgt || lostGpsHgt || lostExtNavHgt || lostRngBcnHgt) { + bool fallback_to_baro = lostRngHgt || lostGpsHgt || lostRngBcnHgt; +#if EK3_FEATURE_EXTERNAL_NAV + bool lostExtNavHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) && !extNavDataIsFresh); + fallback_to_baro |= lostExtNavHgt; +#endif + if (fallback_to_baro) { activeHgtSource = AP_NavEKF_Source::SourceZ::BARO; } @@ -1073,12 +1106,15 @@ void NavEKF3_core::selectHeightForFusion() } // Select the height measurement source +#if EK3_FEATURE_EXTERNAL_NAV if (extNavDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) { hgtMea = -extNavDataDelayed.pos.z; velPosObs[5] = -hgtMea; posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); fuseHgtData = true; - } else if (rangeDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) { + } else +#endif // EK3_FEATURE_EXTERNAL_NAV + if (rangeDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) { // using range finder data // correct for tilt using a flat earth model if (prevTnb.c.z >= 0.7) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 993fa80f39..ca7e8eefe2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -136,6 +136,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) if(dal.beacon() && !storedRangeBeacon.init(imu_buffer_length+1)) { return false; } +#if EK3_FEATURE_EXTERNAL_NAV if (frontend->sources.ext_nav_enabled() && !storedExtNav.init(extnav_buffer_length)) { return false; } @@ -145,6 +146,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) if(frontend->sources.ext_nav_enabled() && !storedExtNavYawAng.init(extnav_buffer_length)) { return false; } +#endif // EK3_FEATURE_EXTERNAL_NAV if(!storedIMU.init(imu_buffer_length)) { return false; } @@ -393,6 +395,7 @@ void NavEKF3_core::InitialiseVariables() memset(&yawAngDataNew, 0, sizeof(yawAngDataNew)); memset(&yawAngDataDelayed, 0, sizeof(yawAngDataDelayed)); +#if EK3_FEATURE_EXTERNAL_NAV // external nav data fusion extNavDataDelayed = {}; extNavMeasTime_ms = 0; @@ -403,6 +406,7 @@ void NavEKF3_core::InitialiseVariables() extNavVelToFuse = false; useExtNavVel = false; extNavVelMeasTime_ms = 0; +#endif // zero data buffers storedIMU.reset(); @@ -416,8 +420,10 @@ void NavEKF3_core::InitialiseVariables() storedBodyOdm.reset(); storedWheelOdm.reset(); #endif +#if EK3_FEATURE_EXTERNAL_NAV storedExtNav.reset(); storedExtNavVel.reset(); +#endif // initialise pre-arm message dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising"); @@ -458,7 +464,9 @@ void NavEKF3_core::InitialiseVariablesMag() magFieldLearned = false; storedMag.reset(); storedYawAng.reset(); +#if EK3_FEATURE_EXTERNAL_NAV storedExtNavYawAng.reset(); +#endif needMagBodyVarReset = false; needEarthBodyVarReset = false; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 271236dced..9e1d64e52a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -1316,6 +1316,7 @@ private: bool onGroundNotMoving; // true when on the ground and not moving uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec) +#if EK3_FEATURE_EXTERNAL_NAV // external navigation fusion EKF_obs_buffer_t storedExtNav; // external navigation data buffer ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon @@ -1327,13 +1328,14 @@ private: ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec) bool extNavVelToFuse; // true when there is new external navigation velocity to fuse - bool useExtNavVel; // true if external nav velocity should be used Vector3f extNavVelInnov; // external nav velocity innovations Vector3f extNavVelVarInnov; // external nav velocity innovation variances uint32_t extNavVelInnovTime_ms; // system time that external nav velocity innovations were recorded (to detect timeouts) EKF_obs_buffer_t storedExtNavYawAng; // external navigation yaw angle buffer yaw_elements extNavYawAngDataDelayed; // external navigation yaw angle at the fusion time horizon uint32_t last_extnav_yaw_fusion_ms; // system time that external nav yaw was last fused +#endif // EK3_FEATURE_EXTERNAL_NAV + bool useExtNavVel; // true if external nav velocity should be used // flags indicating severe numerical errors in innovation variance calculation for different fusion operations struct { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h index 591d8d8073..0952c23eff 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_feature.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_feature.h @@ -10,4 +10,12 @@ #define EK3_FEATURE_ALL APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) || APM_BUILD_TYPE(APM_BUILD_Replay) // body odomotry (which includes wheel encoding) on rover or 2M boards +#ifndef EK3_FEATURE_BODY_ODOM #define EK3_FEATURE_BODY_ODOM EK3_FEATURE_ALL || APM_BUILD_TYPE(APM_BUILD_Rover) || BOARD_FLASH_SIZE > 1024 +#endif + +// external navigation on 2M boards +#ifndef EK3_FEATURE_EXTERNAL_NAV +#define EK3_FEATURE_EXTERNAL_NAV EK3_FEATURE_ALL || BOARD_FLASH_SIZE > 1024 +#endif +