diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 11ed9e9911..ad0e6ab3b5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -642,7 +642,7 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Group: SRC_ // @Path: ../AP_NavEKF/AP_NavEKF_Source.cpp - AP_SUBGROUPINFO(_sources, "SRC", 63, NavEKF3, AP_NavEKF_Source), + AP_SUBGROUPINFO(sources, "SRC", 63, NavEKF3, AP_NavEKF_Source), AP_GROUPEND }; @@ -672,7 +672,7 @@ bool NavEKF3::InitialiseFilter(void) _framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5)); // initialise sources - _sources.init(); + sources.init(); #if APM_BUILD_TYPE(APM_BUILD_Replay) if (ins.get_accel_count() == 0) { @@ -899,7 +899,7 @@ void NavEKF3::UpdateFilter(void) } // align position of inactive sources to ahrs - _sources.align_inactive_sources(); + sources.align_inactive_sources(); } /* @@ -996,7 +996,7 @@ void NavEKF3::resetCoreErrors(void) // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary void NavEKF3::setPosVelYawSource(uint8_t source_set_idx) { - _sources.setPosVelYawSource(source_set_idx); + sources.setPosVelYawSource(source_set_idx); } // Check basic filter health metrics and return a consolidated health status @@ -1012,7 +1012,7 @@ bool NavEKF3::healthy(void) const bool NavEKF3::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { // check source configuration - if (!_sources.pre_arm_check(failure_msg, failure_msg_len)) { + if (!sources.pre_arm_check(failure_msg, failure_msg_len)) { return false; } @@ -1319,7 +1319,7 @@ bool NavEKF3::setOriginLLH(const Location &loc) if (!core) { return false; } - if (_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) { + if (sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) { // we don't allow setting of the EKF origin if using GPS to prevent // accidental setting of EKF origin with invalid position or height GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 refusing set origin"); @@ -1601,7 +1601,7 @@ bool NavEKF3::getDataEKFGSF(int8_t instance, float &yaw_composite, float &yaw_co void NavEKF3::convert_parameters() { // exit immediately if param conversion has been done before - if (_sources.any_params_configured_in_storage()) { + if (sources.any_params_configured_in_storage()) { return; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index f6ac326835..9dfb30d151 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -625,5 +625,5 @@ private: void Log_Write_GSF(uint8_t core, uint64_t time_us) const; // position, velocity and yaw source control - AP_NavEKF_Source _sources; + AP_NavEKF_Source sources; }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index dbe91fe303..673a2920a0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -445,7 +445,7 @@ bool NavEKF3_core::readyToUseOptFlow(void) const return false; } - if (!frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) { + if (!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) { return false; } @@ -456,8 +456,8 @@ bool NavEKF3_core::readyToUseOptFlow(void) const // return true if the filter is ready to start using body frame odometry measurements bool NavEKF3_core::readyToUseBodyOdm(void) const { - if (!frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV) && - !frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) { + if (!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV) && + !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) { return false; } @@ -477,7 +477,7 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const // return true if the filter to be ready to use gps bool NavEKF3_core::readyToUseGPS(void) const { - if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS) { + if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS) { return false; } @@ -487,7 +487,7 @@ bool NavEKF3_core::readyToUseGPS(void) const // return true if the filter to be ready to use the beacon range measurements bool NavEKF3_core::readyToUseRangeBeacon(void) const { - if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::BEACON) { + if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::BEACON) { return false; } @@ -497,7 +497,7 @@ 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 (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) { + if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV) { return false; } @@ -508,7 +508,7 @@ bool NavEKF3_core::readyToUseExtNav(void) const bool NavEKF3_core::use_compass(void) const { const auto *compass = dal.get_compass(); - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if ((yaw_source != AP_NavEKF_Source::SourceYaw::COMPASS) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) { // not using compass as a yaw source @@ -523,7 +523,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 { - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass()) { return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000; } @@ -544,7 +544,7 @@ 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) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) { + if ((PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) { // reject attempt to set origin if GPS is being used return false; } @@ -590,7 +590,7 @@ void NavEKF3_core::checkGyroCalStatus(void) { // check delta angle bias variances const float delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg)); - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if (!use_compass() && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL) && (yaw_source != AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK)) { // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference // which can make this check fail @@ -633,7 +633,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->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS) && !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) @@ -650,7 +650,7 @@ void NavEKF3_core::updateFilterStatus(void) filterStatus.flags.takeoff = expectTakeoff; // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE); - filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy + filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy filterStatus.flags.gps_quality_good = gpsGoodToAlign; filterStatus.flags.initalized = filterStatus.flags.initalized || healthy(); } @@ -663,8 +663,8 @@ void NavEKF3_core::runYawEstimatorPrediction() } // ensure GPS is used for horizontal position and velocity - if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS || - !frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS)) { + if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS || + !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS)) { return; } @@ -689,8 +689,8 @@ void NavEKF3_core::runYawEstimatorCorrection() return; } // ensure GPS is used for horizontal position and velocity - if (frontend->_sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS || - !frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS)) { + if (frontend->sources.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS || + !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS)) { return; } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index 3046a3269e..ed7009047f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -200,7 +200,7 @@ void NavEKF3_core::SelectMagFusion() magFusePerformed = false; // get default yaw source - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); if (yaw_source != yaw_source_last) { yaw_source_last = yaw_source; yaw_source_reset = true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index c354cc169d..d40a97eebe 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -616,7 +616,7 @@ void NavEKF3_core::readGpsData() } // Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly - if (gps.have_vertical_velocity(selected_gps) && frontend->_sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) { + if (gps.have_vertical_velocity(selected_gps) && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) { useGpsVertVel = true; } else { useGpsVertVel = false; @@ -1279,7 +1279,7 @@ float NavEKF3_core::MagDeclination(void) const */ void NavEKF3_core::updateMovementCheck(void) { - const AP_NavEKF_Source::SourceYaw yaw_source = frontend->_sources.getYawSource(); + const AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(); const bool runCheck = onGround && (yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL || yaw_source == AP_NavEKF_Source::SourceYaw::EXTERNAL_COMPASS_FALLBACK || !use_compass()); if (!runCheck) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 5aea038492..b09e737e05 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -52,7 +52,7 @@ void NavEKF3_core::SelectFlowFusion() // Fuse optical flow data into the main filter if (flowDataToFuse && tiltOK) { - if ((frontend->_flowUse == FLOW_USE_NAV) && frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) { + if ((frontend->_flowUse == FLOW_USE_NAV) && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW)) { // 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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 1fd31ae224..439e8656a3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -119,7 +119,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->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW) && (PV_AidingMode == AID_RELATIVE) && flowDataValid) { + if (frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW) && (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 auto *_rng = dal.rangefinder(); if (_rng == nullptr) { @@ -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->_sources.getPosZSource() != AP_NavEKF_Source::SourceZ::RANGEFINDER) { + if (frontend->sources.getPosZSource() != AP_NavEKF_Source::SourceZ::RANGEFINDER) { height -= terrainState; } return true; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 31a7c307de..76e22f75c7 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -245,7 +245,7 @@ void NavEKF3_core::ResetHeight(void) // Reset the vertical velocity state using GPS vertical velocity if we are airborne // Check that GPS vertical velocity data is available and can be used - if (inFlight && !gpsNotAvailable && frontend->_sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) { + if (inFlight && !gpsNotAvailable && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !frontend->inhibitGpsVertVelUse) { stateStruct.velocity.z = gpsDataNew.vel.z; } else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) { stateStruct.velocity.z = extNavVelDelayed.vel.z; @@ -448,7 +448,7 @@ void NavEKF3_core::SelectVelPosFusion() } // detect position source changes. Trigger position reset if position source is valid - AP_NavEKF_Source::SourceXY pos_source = frontend->_sources.getPosXYSource(); + AP_NavEKF_Source::SourceXY pos_source = frontend->sources.getPosXYSource(); if (pos_source != pos_source_last) { pos_source_reset = (pos_source != AP_NavEKF_Source::SourceXY::NONE); pos_source_last = pos_source; @@ -459,10 +459,10 @@ void NavEKF3_core::SelectVelPosFusion() fuseVelData = false; // Determine if we need to fuse position and velocity data on this time step - if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) { + if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS)) { // Don't fuse velocity data if GPS doesn't support it - fuseVelData = frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS); + fuseVelData = frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS); fusePosData = true; extNavUsedForPos = false; @@ -475,7 +475,7 @@ void NavEKF3_core::SelectVelPosFusion() } velPosObs[3] = gpsDataDelayed.pos.x; velPosObs[4] = gpsDataDelayed.pos.y; - } else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV)) { + } else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV)) { // use external nav system for horizontal position extNavUsedForPos = true; fusePosData = true; @@ -485,7 +485,7 @@ void NavEKF3_core::SelectVelPosFusion() // fuse external navigation velocity data if available // extNavVelDelayed is already corrected for sensor position - if (extNavVelToFuse && frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) { + if (extNavVelToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) { fuseVelData = true; velPosObs[0] = extNavVelDelayed.vel.x; velPosObs[1] = extNavVelDelayed.vel.y; @@ -501,7 +501,7 @@ void NavEKF3_core::SelectVelPosFusion() selectHeightForFusion(); // if we are using GPS, check for a change in receiver and reset position and height - if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) && (gpsDataDelayed.sensor_idx != last_gps_idx || pos_source_reset)) { + if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS) && (gpsDataDelayed.sensor_idx != last_gps_idx || pos_source_reset)) { // mark a source reset as consumed pos_source_reset = false; @@ -518,7 +518,7 @@ void NavEKF3_core::SelectVelPosFusion() } // check for external nav position reset - if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || pos_source_reset)) { + if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || pos_source_reset)) { // mark a source reset as consumed pos_source_reset = false; ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y); @@ -658,7 +658,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->_sources.getPosZSource() != AP_NavEKF_Source::SourceZ::GPS)) { + 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]; @@ -720,7 +720,7 @@ void NavEKF3_core::FuseVelPosNED() // test velocity measurements uint8_t imax = 2; // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data - if ((!frontend->_sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) { + if ((!frontend->sources.haveVelZSource() || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) && !useExtNavVel) { imax = 1; } float innovVelSumSq = 0; // sum of squares of velocity innovations @@ -993,13 +993,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->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV)) { + if (extNavUsedForPos && (frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV)) { // always use external navigation as the height source if using for position. activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV; - } else if ((frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) { + } else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) { // user has specified the range finder as a primary height source 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) { + } 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; @@ -1028,22 +1028,22 @@ void NavEKF3_core::selectHeightForFusion() */ if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) { // cannot trust terrain or range finder so stop using range finder height - if (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) { + 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) { + } 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 = AP_NavEKF_Source::SourceZ::RANGEFINDER; } - } else if (frontend->_sources.getPosZSource() == AP_NavEKF_Source::SourceZ::BARO) { + } 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) { + } 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) { + } 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) { + } else if ((frontend->sources.getPosZSource() == AP_NavEKF_Source::SourceZ::EXTNAV) && extNavDataIsFresh) { activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV; } @@ -1807,7 +1807,7 @@ void NavEKF3_core::SelectBodyOdomFusion() // Check for body odometry data (aka visual position delta) at the fusion time horizon const bool bodyOdomDataToFuse = storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms); - if (bodyOdomDataToFuse && frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) { + if (bodyOdomDataToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV)) { // Fuse data into the main filter FuseBodyVel(); @@ -1815,7 +1815,7 @@ void NavEKF3_core::SelectBodyOdomFusion() // Check for wheel encoder data at the fusion time horizon const bool wheelOdomDataToFuse = storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms); - if (wheelOdomDataToFuse && frontend->_sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) { + if (wheelOdomDataToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) { // check if the delta time is too small to calculate a velocity if (wheelOdmDataDelayed.delTime > EKF_TARGET_DT) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp index a28f5ce437..70d3c81658 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp @@ -14,7 +14,7 @@ void NavEKF3_core::SelectRngBcnFusion() // Determine if we need to fuse range beacon data on this time step if (rngBcnDataToFuse) { if (PV_AidingMode == AID_ABSOLUTE) { - if ((frontend->_sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::BEACON) && rngBcnAlignmentCompleted) { + if ((frontend->sources.getPosXYSource() == AP_NavEKF_Source::SourceXY::BEACON) && rngBcnAlignmentCompleted) { if (!bcnOriginEstInit) { bcnOriginEstInit = true; bcnPosOffsetNED.x = receiverPos.x - stateStruct.position.x; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index 0f0b7e73d3..12d6c2105b 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -83,14 +83,14 @@ void NavEKF3_core::calcGpsGoodToAlign(void) gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); - } else if (frontend->_sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !gps.have_vertical_velocity(preferred_gps)) { + } else if (frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) && !gps.have_vertical_velocity(preferred_gps)) { // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail gpsVertVelFail = true; // if we have a 3D fix with no vertical velocity and // EK3_GPS_TYPE=0 then change it to 1. It means the GPS is not // capable of giving a vertical velocity if (gps.status(preferred_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) { - frontend->_sources.setVelZSource(AP_NavEKF_Source::SourceZ::NONE); + frontend->sources.setVelZSource(AP_NavEKF_Source::SourceZ::NONE); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EK3: Changed EK3_GPS_TYPE to 1"); } } else { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 9a43f33e8b..04950b2aa0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -46,7 +46,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index) )))); // GPS sensing can have large delays and should not be included if disabled - if (frontend->_sources.usingGPS()) { + if (frontend->sources.usingGPS()) { // Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay float gps_delay_sec = 0; if (!dal.gps().get_lag(selected_gps, gps_delay_sec)) { @@ -535,8 +535,8 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) ResetHeight(); // initialise sources - pos_source_last = frontend->_sources.getPosXYSource(); - yaw_source_last = frontend->_sources.getYawSource(); + pos_source_last = frontend->sources.getPosXYSource(); + yaw_source_last = frontend->sources.getYawSource(); // define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, dal.get_home().lat);