diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index bb916d5882..9140701c45 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -585,9 +585,8 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { AP_GROUPEND }; -NavEKF2::NavEKF2(const AP_AHRS *ahrs, const RangeFinder &rng) : - _ahrs(ahrs), - _rng(rng) +NavEKF2::NavEKF2(const AP_AHRS *ahrs) : + _ahrs(ahrs) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 7cd62f01d7..73625091dd 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -28,7 +28,6 @@ #include #include #include -#include #include class NavEKF2_core; @@ -38,7 +37,7 @@ class NavEKF2 { friend class NavEKF2_core; public: - NavEKF2(const AP_AHRS *ahrs, const RangeFinder &rng); + NavEKF2(const AP_AHRS *ahrs); /* Do not allow copies */ NavEKF2(const NavEKF2 &other) = delete; @@ -361,7 +360,6 @@ private: NavEKF2_core *core = nullptr; bool core_malloc_failed; const AP_AHRS *_ahrs; - const RangeFinder &_rng; uint32_t _frameTimeUsec; // time per IMU frame uint8_t _framesPerPrediction; // expected number of IMU frames per prediction diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index c8bfb5a952..19460e1aa6 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -28,11 +29,16 @@ void NavEKF2_core::readRangeFinder(void) // get theoretical correct range when the vehicle is on the ground // don't allow range to go below 5cm because this can cause problems with optical flow processing - rngOnGnd = MAX(frontend->_rng.ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f); + const RangeFinder *_rng = AP::rangefinder(); + if (_rng == nullptr) { + return; + } + + rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f); // read range finder at 20Hz // TODO better way of knowing if it has new data - if ((imuSampleTime_ms - lastRngMeasTime_ms) > 50) { + if (_rng && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) { // reset the timer used to control the measurement rate lastRngMeasTime_ms = imuSampleTime_ms; @@ -41,7 +47,7 @@ void NavEKF2_core::readRangeFinder(void) // use data from two range finders if available for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) { - AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(sensorIndex); + AP_RangeFinder_Backend *sensor = _rng->get_backend(sensorIndex); if (sensor == nullptr) { continue; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index ccb958b05b..acf84b718a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include @@ -103,7 +104,12 @@ bool NavEKF2_core::getHeightControlLimit(float &height) const // only ask for limiting if we are doing optical flow only navigation 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 - height = MAX(float(frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270)) * 0.007f - 1.0f, 1.0f); + const RangeFinder *_rng = AP::rangefinder(); + if (_rng == nullptr) { + // we really, really shouldn't be here. + return false; + } + 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) { height -= terrainState; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index c69da152ef..0c454529df 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -4,6 +4,7 @@ #include "AP_NavEKF2_core.h" #include #include +#include #include #include #include @@ -813,8 +814,9 @@ void NavEKF2_core::selectHeightForFusion() // correct range data for the body frame position offset relative to the IMU // the corrected reading is the reading that would have been taken if the sensor was // co-located with the IMU - if (rangeDataToFuse) { - AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(rangeDataDelayed.sensor_idx); + const RangeFinder *_rng = AP::rangefinder(); + if (_rng && rangeDataToFuse) { + const AP_RangeFinder_Backend *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx); if (sensor != nullptr) { Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset; if (!posOffsetBody.is_zero()) { @@ -832,13 +834,13 @@ void NavEKF2_core::selectHeightForFusion() if (extNavUsedForPos) { // always use external vision as the height source if using for position. activeHgtSource = HGT_SOURCE_EV; - } else if (((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) { + } 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; } else { // determine if we are above or below the height switch region - float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt; + float rangeMaxUse = 1e-4f * (float)_rng->max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt; bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse; bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;