aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_WPNav: use rangefinder for terrain offset even if terrain not available

Browse Source
This commit is contained in:
Peter Barker 2018-06-05 18:40:01 +10:00 committed by Randy Mackay
parent ab68e4fe8b
commit fa0b315374
1 changed files with 2 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -973,17 +973,16 @@ void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector
// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool AC_WPNav::get_terrain_offset(float& offset_cm) bool AC_WPNav::get_terrain_offset(float& offset_cm)
{ {
#if AP_TERRAIN_AVAILABLE
// use range finder if connected // use range finder if connected
if (_rangefinder_available && _rangefinder_use) { if (_rangefinder_available && _rangefinder_use) {
if (_rangefinder_healthy) { if (_rangefinder_healthy) {
offset_cm = _inav.get_altitude() - _rangefinder_alt_cm; offset_cm = _inav.get_altitude() - _rangefinder_alt_cm;
return true; return true;
} else { }
return false; return false;
} }
}
#if AP_TERRAIN_AVAILABLE
// use terrain database // use terrain database
float terr_alt = 0.0f; float terr_alt = 0.0f;
if (_terrain != nullptr && _terrain->height_above_terrain(terr_alt, true)) { if (_terrain != nullptr && _terrain->height_above_terrain(terr_alt, true)) {