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

AC_WPNav: add get_terrain_source and rename existing method

Browse Source 
this allows RTL to determine which source of terrain data will be used which is required when building the return path
This commit is contained in:
Randy Mackay 2019-12-12 20:04:15 +09:00
parent c545171cec
commit 3ca28e27c6
2 changed files with 41 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -95,6 +95,23 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
_wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN);
}
// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const
{
// use range finder if connected
if (_rangefinder_available && _rangefinder_use) {
return AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER;
}
#if AP_TERRAIN_AVAILABLE
if ((_terrain != nullptr) && _terrain->enabled()) {
return AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE;
} else {
return AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE;
}
#else
return AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE;
#endif
}
///
/// waypoint navigation
@ -941,23 +958,28 @@ 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)
bool AC_WPNav::get_terrain_offset(float& offset_cm)
{
// use range finder if connected
if (_rangefinder_available && _rangefinder_use) {
// calculate offset based on source (rangefinder or terrain database)
switch (get_terrain_source()) {
case AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE:
return false;
case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
if (_rangefinder_healthy) {
offset_cm = _inav.get_altitude() - _rangefinder_alt_cm;
return true;
}
return false;
case AC_WPNav::TerrainSource::TERRAIN_FROM_TERRAINDATABASE:
#if AP_TERRAIN_AVAILABLE
float terr_alt = 0.0f;
if (_terrain != nullptr && _terrain->height_above_terrain(terr_alt, true)) {
offset_cm = _inav.get_altitude() - (terr_alt * 100.0f);
return true;
}
#endif
return false;
}
#if AP_TERRAIN_AVAILABLE
// use terrain database
float terr_alt = 0.0f;
if (_terrain != nullptr && _terrain->height_above_terrain(terr_alt, true)) {
offset_cm = _inav.get_altitude() - (terr_alt * 100.0f);
return true;
}
#endif
// we should never get here but just in case
return false;
}

10
libraries/AC_WPNav/AC_WPNav.h
View File

@ -55,7 +55,15 @@ public:
void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; }
// return true if range finder may be used for terrain following
bool rangefinder_used() const { return _rangefinder_use && _rangefinder_healthy; }
bool rangefinder_used_and_healthy() const { return _rangefinder_use && _rangefinder_healthy; }
// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
enum class TerrainSource {
TERRAIN_UNAVAILABLE,
TERRAIN_FROM_RANGEFINDER,
TERRAIN_FROM_TERRAINDATABASE,
};
AC_WPNav::TerrainSource get_terrain_source() const;
///
/// waypoint controller