Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: Provide terrain altitude for surface tracking

Browse Source
This commit is contained in:
Leonard Hall 2023-03-04 11:31:13 +10:30 committed by Peter Barker
parent 518fece88d
commit 6344faeb29
4 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -358,7 +358,7 @@ bool AC_Circle::get_terrain_offset(float& offset_cm)
return false; return false;
case AC_Circle::TerrainSource::TERRAIN_FROM_RANGEFINDER: case AC_Circle::TerrainSource::TERRAIN_FROM_RANGEFINDER:
if (_rangefinder_healthy) { if (_rangefinder_healthy) {
offset_cm = _inav.get_position_z_up_cm() - _rangefinder_alt_cm; offset_cm = _rangefinder_terrain_offset_cm;
return true; return true;
} }
return false; return false;

7
libraries/AC_WPNav/AC_Circle.h
View File

@ -89,8 +89,9 @@ public:
/// true if pilot control of radius and turn rate is enabled /// true if pilot control of radius and turn rate is enabled
bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; } bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; }
/// provide rangefinder altitude /// provide rangefinder based terrain offset
void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; } /// terrain offset is the terrain's height above the EKF origin
void set_rangefinder_terrain_offset(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;}
/// check for a change in the radius params /// check for a change in the radius params
void check_param_change(); void check_param_change();
@ -159,5 +160,5 @@ private:
bool _terrain_alt; // true if _center.z is alt-above-terrain, false if alt-above-ekf-origin bool _terrain_alt; // true if _center.z is alt-above-terrain, false if alt-above-ekf-origin
bool _rangefinder_available; // true if range finder could be used bool _rangefinder_available; // true if range finder could be used
bool _rangefinder_healthy; // true if range finder is healthy bool _rangefinder_healthy; // true if range finder is healthy
float _rangefinder_alt_cm; // latest rangefinder altitude float _rangefinder_terrain_offset_cm; // latest rangefinder based terrain offset (e.g. terrain's height above EKF origin)
}; };

2
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -629,7 +629,7 @@ bool AC_WPNav::get_terrain_offset(float& offset_cm)
return false; return false;
case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER: case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
if (_rangefinder_healthy) { if (_rangefinder_healthy) {
offset_cm = _inav.get_position_z_up_cm() - _rangefinder_alt_cm; offset_cm = _rangefinder_terrain_offset_cm;
return true; return true;
} }
return false; return false;

7
libraries/AC_WPNav/AC_WPNav.h
View File

@ -22,8 +22,9 @@ public:
/// Constructor /// Constructor
AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control);
/// provide rangefinder altitude /// provide rangefinder based terrain offset
void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; } /// terrain offset is the terrain's height above the EKF origin
void set_rangefinder_terrain_offset(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;}
// return true if range finder may be used for terrain following // return true if range finder may be used for terrain following
bool rangefinder_used() const { return _rangefinder_use; } bool rangefinder_used() const { return _rangefinder_use; }
@ -280,5 +281,5 @@ protected:
bool _rangefinder_available; // true if rangefinder is enabled (user switch can turn this true/false) bool _rangefinder_available; // true if rangefinder is enabled (user switch can turn this true/false)
AP_Int8 _rangefinder_use; // parameter that specifies if the range finder should be used for terrain following commands AP_Int8 _rangefinder_use; // parameter that specifies if the range finder should be used for terrain following commands
bool _rangefinder_healthy; // true if rangefinder distance is healthy (i.e. between min and maximum) bool _rangefinder_healthy; // true if rangefinder distance is healthy (i.e. between min and maximum)
float _rangefinder_alt_cm; // latest distance from the rangefinder float _rangefinder_terrain_offset_cm; // latest rangefinder based terrain offset (e.g. terrain's height above EKF origin)
}; };