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

AC_WPNav: added WP_RFND_USE parameter 

allows disable of rangefinder use for terrain following
This commit is contained in:
Andrew Tridgell 2016-08-18 20:56:03 +10:00
parent 569a7a4abd
commit c87fa13e77
2 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -97,6 +97,13 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
// @User: Advanced
AP_GROUPINFO("LOIT_MINA", 9, AC_WPNav, _loiter_accel_min_cmss, WPNAV_LOITER_ACCEL_MIN),
// @Param: RFND_USE
// @DisplayName: Use rangefinder for terrain following
// @Description: This controls the use of a rangefinder for terrain following
// @Values: 0:Disable,1:Enable
// @User: Advanced
AP_GROUPINFO("RFND_USE", 10, AC_WPNav, _rangefinder_use, 1),
AP_GROUPEND
};
@ -1153,7 +1160,7 @@ bool AC_WPNav::get_terrain_offset(float& offset_cm)
{
#if AP_TERRAIN_AVAILABLE
// use range finder if connected
if (_rangefinder_use) {
if (_rangefinder_available && _rangefinder_use) {
if (_rangefinder_healthy) {
offset_cm = _inav.get_altitude() - _rangefinder_alt_cm;
return true;

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -64,7 +64,7 @@ public:
void set_avoidance(AC_Avoid* avoid_ptr) { _avoid = avoid_ptr; }
/// provide rangefinder altitude
void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_use = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; }
void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; }
///
/// loiter controller
@ -362,7 +362,8 @@ protected:
// terrain following variables
bool _terrain_alt = false; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
bool _ekf_origin_terrain_alt_set = false;
bool _rangefinder_use = false;
bool _rangefinder_available;
AP_Int8 _rangefinder_use;
bool _rangefinder_healthy = false;
float _rangefinder_alt_cm = 0.0f;
};