From 3f6a734a71c406d7fe1dc821d3f1e6926acac483 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 9 Feb 2017 19:27:52 +0900 Subject: [PATCH] AP_NavEKF3: only use downward facing rangefinder --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 6 +++--- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 7bc74e4e7e..52ea4abdf2 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -22,8 +22,8 @@ void NavEKF3_core::readRangeFinder(void) uint8_t maxIndex; uint8_t minIndex; // get theoretical correct range when the vehicle is on the ground - // don't allow range to go below 5cm becasue this can cause problems with optical flow processing - rngOnGnd = MAX(frontend->_rng.ground_clearance_cm() * 0.01f, 0.05f); + // 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); // limit update rate to maximum allowed by data buffers if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) { @@ -35,7 +35,7 @@ void NavEKF3_core::readRangeFinder(void) // use data from two range finders if available for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) { - if (frontend->_rng.status(sensorIndex) == RangeFinder::RangeFinder_Good) { + if ((frontend->_rng.get_orientation(sensorIndex) == ROTATION_PITCH_270) && (frontend->_rng.status(sensorIndex) == RangeFinder::RangeFinder_Good)) { rngMeasIndex[sensorIndex] ++; if (rngMeasIndex[sensorIndex] > 2) { rngMeasIndex[sensorIndex] = 0; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 8d104eaa71..3ea89317f8 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -103,7 +103,7 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const // only ask for limiting if we are doing optical flow navigation if (frontend->_fusionModeGPS == 3) { // 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()) * 0.007f - 1.0f, 1.0f); + height = MAX(float(frontend->_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_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index 4c29a7fdab..1a6de1fc0e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -696,7 +696,7 @@ void NavEKF3_core::selectHeightForFusion() 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() * (float)frontend->_useRngSwHgt; + float rangeMaxUse = 1e-4f * (float)frontend->_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;