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

AC_WPNav: support rangefinder for terrain following

Browse Source
This commit is contained in:
Randy Mackay 2016-04-28 12:29:35 +09:00
parent e18bf3af56
commit 2bafc36ded
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -1155,6 +1155,17 @@ void AC_WPNav::calc_spline_pos_vel(float spline_time, Vector3f& position, Vector
bool AC_WPNav::get_terrain_offset(float& offset_cm) bool AC_WPNav::get_terrain_offset(float& offset_cm)
{ {
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
// use range finder if connected
if (_rangefinder_use) {
if (_rangefinder_healthy) {
offset_cm = _inav.get_altitude() - _rangefinder_alt_cm;
return true;
} else {
return false;
}
}
// use terrain database
float terr_alt = 0.0f; float terr_alt = 0.0f;
if (_terrain != NULL && _terrain->height_above_terrain(terr_alt, true)) { if (_terrain != NULL && _terrain->height_above_terrain(terr_alt, true)) {
offset_cm = _inav.get_altitude() - (terr_alt * 100.0f); offset_cm = _inav.get_altitude() - (terr_alt * 100.0f);

8
libraries/AC_WPNav/AC_WPNav.h
View File

@ -40,6 +40,8 @@
#define WPNAV_YAW_DIST_MIN 200 // minimum track length which will lead to target yaw being updated to point at next waypoint. Under this distance the yaw target will be frozen at the current heading #define WPNAV_YAW_DIST_MIN 200 // minimum track length which will lead to target yaw being updated to point at next waypoint. Under this distance the yaw target will be frozen at the current heading
#define WPNAV_RANGEFINDER_FILT_Z 0.25f // range finder distance filtered at 0.25hz
class AC_WPNav class AC_WPNav
{ {
public: public:
@ -57,6 +59,9 @@ public:
/// provide pointer to terrain database /// provide pointer to terrain database
void set_terrain(AP_Terrain* terrain_ptr) { _terrain = terrain_ptr; } void set_terrain(AP_Terrain* terrain_ptr) { _terrain = terrain_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; }
/// ///
/// loiter controller /// loiter controller
/// ///
@ -356,4 +361,7 @@ protected:
// terrain following variables // terrain following variables
bool _terrain_alt = false; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin 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 _ekf_origin_terrain_alt_set = false;
bool _rangefinder_use = false;
bool _rangefinder_healthy = false;
float _rangefinder_alt_cm = 0.0f;
}; };