From 33ec14ce7053ec4773e8858d25c2655da5f5f4a6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 20 Jul 2021 13:40:14 +0900 Subject: [PATCH] AC_WPNav: add TER_MARGIN param --- libraries/AC_WPNav/AC_WPNav.cpp | 8 ++++++++ libraries/AC_WPNav/AC_WPNav.h | 4 ++++ 2 files changed, 12 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index cd220949b9..a942df3c03 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -75,6 +75,14 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @User: Standard 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 }; diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 52887787e8..504dfacc05 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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) 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 // returns false if conversion failed (likely because terrain data was not available) 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_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 _terrain_margin; // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin // scurve SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory