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

AC_WPNav: add TER_MARGIN param

This commit is contained in:
Randy Mackay 2021-07-20 13:40:14 +09:00 committed by Andrew Tridgell
parent 2dea0fa966
commit 7a09ac1aa6
2 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -75,6 +75,14 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("JERK", 11, AC_WPNav, _wp_jerk, 1.0f), AP_GROUPINFO("JERK", 11, AC_WPNav, _wp_jerk, 1.0f),
// @Param: TER_MARGIN
// @DisplayName: Waypoint Terrain following altitude margin
// @Description: Waypoint Terrain following altitude margin. Vehicle will stop if distance from target altitude is larger than this margin (in meters)
// @Units: m
// @Range: 0.1 100
// @User: Advanced
AP_GROUPINFO("TER_MARGIN", 12, AC_WPNav, _terrain_margin, 10.0),
AP_GROUPEND AP_GROUPEND
}; };

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -52,6 +52,9 @@ public:
// 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 get_terrain_offset(float& offset_cm); bool get_terrain_offset(float& offset_cm);
// return terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin
float get_terrain_margin() const { return MAX(_terrain_margin, 0.1); }
// convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain // convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
// returns false if conversion failed (likely because terrain data was not available) // returns false if conversion failed (likely because terrain data was not available)
bool get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt); bool get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt);
@ -240,6 +243,7 @@ protected:
AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions
AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions
AP_Float _wp_jerk; // maximum jerk used to generate scurve trajectories in m/s/s/s AP_Float _wp_jerk; // maximum jerk used to generate scurve trajectories in m/s/s/s
AP_Float _terrain_margin; // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin
// scurve // scurve
SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory