From 8931e501660de2a481c97cb3f4119012c9216fda Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 4 Jul 2020 12:27:49 +0900 Subject: [PATCH] AP_NavEKF3: integrate Source for alt --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 8 +- libraries/AP_NavEKF3/AP_NavEKF3.h | 1 - libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- .../AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 4 +- .../AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 4 +- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 4 +- .../AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 80 ++++++++++--------- .../AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 10 +-- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 3 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 10 +-- 10 files changed, 60 insertions(+), 66 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 81f9df7820..72195fa2f8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -189,13 +189,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // Height measurement parameters - // @Param: ALT_SOURCE - // @DisplayName: Primary altitude sensor source - // @Description: Primary height sensor used by the EKF. If a sensor other than Baro is selected and becomes unavailable, then the Baro sensor will be used as a fallback. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground in conjunction with EK3_ALT_SOURCE = 0 or 2 (Baro or GPS). NOTE: Setting EK3_ALT_SOURCE = 4 uses external nav system data 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), + // 9 was ALT_SOURCE // @Param: ALT_M_NSE // @DisplayName: Altitude measurement noise (m) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 295fd08c55..75c08a21e2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -460,7 +460,6 @@ private: AP_Int8 _flowDelay_ms; // effective average delay of optical flow measurements rel to IMU (msec) AP_Int16 _rngInnovGate; // Percentage number of standard deviations applied to range finder innovation consistency check AP_Float _maxFlowRate; // Maximum flow rate magnitude that will be accepted by the filter - AP_Int8 _altSource; // Primary alt source. 0 = Baro, 1 = range finder, 2 = GPS, 3 = range beacons, 4 = external nav AP_Float _rngNoise; // Range finder noise : m AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF3 for diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 7d7d3e6aef..34a733e0d5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -600,7 +600,7 @@ void NavEKF3_core::updateFilterStatus(void) bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE))); // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks - bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin; + bool hgtNotAccurate = (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && !validOrigin; // set individual flags filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index da5d0f7633..19f43b1eef 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -765,11 +765,11 @@ void NavEKF3_core::correctEkfOriginHeight() // calculate the variance of our a-priori estimate of the ekf origin height float deltaTime = constrain_float(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f); - if (activeHgtSource == HGT_SOURCE_BARO) { + if (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) { // Use the baro drift rate const float baroDriftRate = 0.05f; ekfOriginHgtVar += sq(baroDriftRate * deltaTime); - } else if (activeHgtSource == HGT_SOURCE_RNG) { + } else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) { // use the worse case expected terrain gradient and vehicle horizontal speed const float maxTerrGrad = 0.25f; ekfOriginHgtVar += sq(maxTerrGrad * norm(stateStruct.velocity.x , stateStruct.velocity.y) * deltaTime); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index d39572a4f4..367319b777 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -32,7 +32,7 @@ void NavEKF3_core::SelectFlowFusion() // Check if the optical flow data is still valid flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000); // check is the terrain offset estimate is still valid - if we are using range finder as the main height reference, the ground is assumed to be at 0 - gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == HGT_SOURCE_RNG); + gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER); // Perform tilt check bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin); // Constrain measurements to zero if takeoff is not detected and the height above ground @@ -80,7 +80,7 @@ void NavEKF3_core::EstimateTerrainOffset() || velHorizSq < 25.0f || (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate)); - if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == HGT_SOURCE_RNG)) { + if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) { // skip update inhibitGndState = true; } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index b6711a591a..14719338f1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -128,7 +128,7 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const } height = MAX(float(_rng->max_distance_cm_orient(ROTATION_PITCH_270)) * 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) { + if (frontend->_sources.getPosZSource() != AP_NavEKF_Source::SourceZ::RANGEFINDER) { height -= terrainState; } return true; @@ -635,7 +635,7 @@ void NavEKF3_core::send_status_report(mavlink_channel_t chan) const // height estimation or optical flow operation. This prevents false alarms at the GCS if a // range finder is fitted for other applications float temp; - if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { + if (((frontend->_useRngSwHgt > 0) && activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { temp = sqrtf(auxRngTestRatio); } else { temp = 0.0f; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index d1da5ad55d..4096561224 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -247,7 +247,7 @@ void NavEKF3_core::ResetHeight(void) // Check that GPS vertical velocity data is available and can be used if (inFlight && !gpsNotAvailable && (frontend->_sources.getVelZSource() == AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) { stateStruct.velocity.z = gpsDataNew.vel.z; - } else if (inFlight && useExtNavVel && (activeHgtSource == HGT_SOURCE_EXTNAV)) { + } else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) { stateStruct.velocity.z = extNavVelDelayed.vel.z; } else if (onGround) { stateStruct.velocity.z = 0.0f; @@ -275,7 +275,7 @@ void NavEKF3_core::ResetHeight(void) // Return true if the height datum reset has been performed bool NavEKF3_core::resetHeightDatum(void) { - if (activeHgtSource == HGT_SOURCE_RNG || !onGround) { + if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER || !onGround) { // only allow resets when on the ground. // If using using rangefinder for height then never perform a // reset of the height datum @@ -489,7 +489,7 @@ void NavEKF3_core::SelectVelPosFusion() ResetPositionNE(gpsDataDelayed.pos.x, gpsDataDelayed.pos.y); // If we are also using GPS as the height reference, reset the height - if (activeHgtSource == HGT_SOURCE_GPS) { + if (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) { ResetPositionD(-hgtMea); } } @@ -499,7 +499,7 @@ void NavEKF3_core::SelectVelPosFusion() // mark a source reset as consumed pos_source_reset = false; ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y); - if (activeHgtSource == HGT_SOURCE_EXTNAV) { + if (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) { ResetPositionD(-hgtMea); } } @@ -635,7 +635,7 @@ void NavEKF3_core::FuseVelPosNED() // if vertical GPS velocity data and an independent 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 && (frontend->_altSource != 2)) { + if (useGpsVertVel && fuseVelData && (frontend->_sources.getPosZSource() != AP_NavEKF_Source::SourceZ::GPS)) { // calculate innovations for height and vertical GPS vel measurements const float hgtErr = stateStruct.position.z - velPosObs[5]; const float velDErr = stateStruct.velocity.z - velPosObs[2]; @@ -810,7 +810,7 @@ void NavEKF3_core::FuseVelPosNED() const float gndMaxBaroErr = 4.0f; const float gndBaroInnovFloor = -0.5f; - if(expectGndEffectTouchdown && activeHgtSource == HGT_SOURCE_BARO) { + if (expectGndEffectTouchdown && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) { // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr // this function looks like this: @@ -970,13 +970,13 @@ void NavEKF3_core::selectHeightForFusion() bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500); const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500); // select height source - if (extNavUsedForPos && (frontend->_altSource == 4)) { + if (extNavUsedForPos && (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV)) { // always use external navigation as the height source if using for position. - activeHgtSource = HGT_SOURCE_EXTNAV; - } else if ((frontend->_altSource == 1) && _rng && rangeFinderDataIsFresh) { + activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV; + } else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) { // user has specified the range finder as a primary height source - activeHgtSource = HGT_SOURCE_RNG; - } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) { + activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER; + } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) || (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) { // determine if we are above or below the height switch region float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; @@ -1003,40 +1003,40 @@ void NavEKF3_core::selectHeightForFusion() * hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height * which cannot be assumed if the vehicle is moving horizontally. */ - if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) { + if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) { // cannot trust terrain or range finder so stop using range finder height - if (frontend->_altSource == 0) { - activeHgtSource = HGT_SOURCE_BARO; - } else if (frontend->_altSource == 2) { - activeHgtSource = HGT_SOURCE_GPS; + if (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) { + activeHgtSource = AP_NavEKF_Source::SourceZ::BARO; + } else if (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) { + activeHgtSource = AP_NavEKF_Source::SourceZ::GPS; } } else if (belowLowerSwHgt && trustTerrain && (prevTnb.c.z >= 0.7f)) { // reliable terrain and range finder so start using range finder height - activeHgtSource = HGT_SOURCE_RNG; + activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER; } - } else if (frontend->_altSource == 0) { - activeHgtSource = HGT_SOURCE_BARO; - } else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) { - activeHgtSource = HGT_SOURCE_GPS; - } else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) { - activeHgtSource = HGT_SOURCE_BCN; - } else if ((frontend->_altSource == 4) && extNavDataIsFresh) { - activeHgtSource = HGT_SOURCE_EXTNAV; + } else if (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) { + activeHgtSource = AP_NavEKF_Source::SourceZ::BARO; + } else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) { + activeHgtSource = AP_NavEKF_Source::SourceZ::GPS; + } else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcnGoodToAlign) { + activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON; + } else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV) && extNavDataIsFresh) { + activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV; } // Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon - bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && !rangeFinderDataIsFresh); - bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000)); - bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && !extNavDataIsFresh); - bool lostRngBcnHgt = ((activeHgtSource == HGT_SOURCE_BCN) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000)); + 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) { - activeHgtSource = HGT_SOURCE_BARO; + activeHgtSource = AP_NavEKF_Source::SourceZ::BARO; } // if there is new baro data to fuse, calculate filtered baro data required by other processes if (baroDataToFuse) { // calculate offset to baro data that enables us to switch to Baro height use during operation - if (activeHgtSource != HGT_SOURCE_BARO) { + if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO) { calcFiltBaroOffset(); } // filtered baro data used to provide a reference for takeoff @@ -1053,19 +1053,19 @@ void NavEKF3_core::selectHeightForFusion() // combined local NED position height and origin height remains consistent with the GPS altitude // This also enables the GPS height to be used as a backup height source if (gpsDataToFuse && - (((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == HGT_SOURCE_BARO)) || - ((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == HGT_SOURCE_RNG))) + (((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) || + ((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER))) ) { correctEkfOriginHeight(); } // Select the height measurement source - if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) { + 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 == HGT_SOURCE_RNG)) { + } else 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) { @@ -1084,7 +1084,7 @@ void NavEKF3_core::selectHeightForFusion() // disable fusion if tilted too far fuseHgtData = false; } - } else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) { + } else if (gpsDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS)) { // using GPS data hgtMea = gpsDataDelayed.hgt; velPosObs[5] = -hgtMea; @@ -1096,7 +1096,7 @@ void NavEKF3_core::selectHeightForFusion() } else { posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f)); } - } else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) { + } else if (baroDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) { // using Baro data hgtMea = baroDataDelayed.hgt - baroHgtOffset; // enable fusion @@ -1117,6 +1117,12 @@ void NavEKF3_core::selectHeightForFusion() fuseHgtData = false; } + // detect changes in source and reset height + if ((activeHgtSource != prevHgtSource) && fuseHgtData) { + prevHgtSource = activeHgtSource; + ResetPositionD(-hgtMea); + } + // 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 || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index 39432e039c..1e0be77dac 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -54,7 +54,7 @@ void NavEKF3_core::FuseRngBcn() // health is set bad until test passed rngBcnHealth = false; - if (activeHgtSource != HGT_SOURCE_BCN) { + if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) { // calculate the vertical offset from EKF datum to beacon datum CalcRangeBeaconPosDownOffset(R_BCN, stateStruct.position, false); } else { @@ -96,7 +96,7 @@ void NavEKF3_core::FuseRngBcn() // are at the same height as the flight vehicle when calculating the observation derivatives // and Kalman gains // TODO - less hacky way of achieving this, preferably using an alternative derivation - if (activeHgtSource != HGT_SOURCE_BCN) { + if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) { t2 = 0.0f; } H_BCN[9] = -t2*t9; @@ -158,7 +158,7 @@ void NavEKF3_core::FuseRngBcn() } // only allow the range observations to modify the vertical states if we are using it as a height reference - if (activeHgtSource == HGT_SOURCE_BCN) { + if (activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) { Kfusion[6] = -t26*(P[6][7]*t4*t9+P[6][8]*t3*t9+P[6][9]*t2*t9); Kfusion[9] = -t26*(t10+P[9][7]*t4*t9+P[9][8]*t3*t9); } else { @@ -324,7 +324,7 @@ void NavEKF3_core::FuseRngBcnStatic() } if (rngBcnAlignmentCompleted) { - if (activeHgtSource != HGT_SOURCE_BCN) { + if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) { // We are using a different height reference for the main EKF so need to estimate a vertical // position offset to be applied to the beacon system that minimises the range innovations // The position estimate should be stable after 100 iterations so we use a simple dual @@ -341,7 +341,7 @@ void NavEKF3_core::FuseRngBcnStatic() } } else { - if (activeHgtSource != HGT_SOURCE_BCN) { + if (activeHgtSource != AP_NavEKF_Source::SourceZ::BEACON) { // The position estimate is not yet stable so we cannot run the 1-state EKF to estimate // beacon system vertical position offset. Instead we initialise the dual hypothesis offset states // using the beacon vertical position, vertical position estimate relative to the beacon origin diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 139be05235..cef871266c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -317,7 +317,8 @@ void NavEKF3_core::InitialiseVariables() delAngBiasLearned = false; memset(&filterStatus, 0, sizeof(filterStatus)); gpsInhibit = false; - activeHgtSource = 0; + activeHgtSource = AP_NavEKF_Source::SourceZ::BARO; + prevHgtSource = activeHgtSource; memset(&rngMeasIndex, 0, sizeof(rngMeasIndex)); memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms)); memset(&storedRngMeas, 0, sizeof(storedRngMeas)); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 8098583909..60f9b29702 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -46,13 +46,6 @@ #define MASK_GPS_VERT_SPD (1<<6) #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_EXTNAV 4 - #define earthRate 0.000072921f // earth rotation rate (rad/sec) // maximum allowed gyro bias (rad/sec) @@ -1306,7 +1299,8 @@ private: } rngBcnFusionReport[4]; // height source selection logic - uint8_t activeHgtSource; // integer defining active height source + AP_NavEKF_Source::SourceZ activeHgtSource; // active height source + AP_NavEKF_Source::SourceZ prevHgtSource; // previous height source used to detect changes in source // Movement detector bool takeOffDetected; // true when takeoff for optical flow navigation has been detected