Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: only use downward facing rangefinder

This commit is contained in:
Randy Mackay 2017-02-09 19:27:52 +09:00
parent 563be389ee
commit 3f6a734a71
3 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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;

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -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;

2
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -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;