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

AP_NavEKF2: only use downward facing rangefinder

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

7
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -23,9 +23,10 @@ void NavEKF2_core::readRangeFinder(void)
uint8_t midIndex;
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);
// read range finder at 20Hz
// TODO better way of knowing if it has new data
@ -38,7 +39,7 @@ void NavEKF2_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_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -104,7 +104,7 @@ bool NavEKF2_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_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -705,7 +705,7 @@ void NavEKF2_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;