From 44f0aef5e99abe42572c3b82a614bea129cc2a91 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Thu, 21 May 2020 14:07:45 +0530 Subject: [PATCH] AP_OAPathPlanner: Add param to reset WP origin while recovering from Bendy --- libraries/AC_Avoidance/AP_OAPathPlanner.cpp | 13 +++++++++++++ libraries/AC_Avoidance/AP_OAPathPlanner.h | 12 ++++++++++-- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 8f499a317b..8c16634272 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -26,6 +26,9 @@ extern const AP_HAL::HAL &hal; // parameter defaults const float OA_LOOKAHEAD_DEFAULT = 15; const float OA_MARGIN_MAX_DEFAULT = 5; +#if APM_BUILD_TYPE(APM_BUILD_Rover) + const int16_t OA_OPTIONS_DEFAULT = 1; +#endif const int16_t OA_UPDATE_MS = 1000; // path planning updates run at 1hz const int16_t OA_TIMEOUT_MS = 3000; // results over 3 seconds old are ignored @@ -61,6 +64,16 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = { // @Path: AP_OADatabase.cpp AP_SUBGROUPINFO(_oadatabase, "DB_", 4, AP_OAPathPlanner, AP_OADatabase), +#if APM_BUILD_TYPE(APM_BUILD_Rover) + // @Param: OPTIONS + // @DisplayName: Options while recovering from Object Avoidance + // @Description: Bitmask which will govern vehicles behaviour while recovering from Obstacle Avoidance (i.e Avoidance is turned off after the path ahead is clear). + // @Values: 0:Vehicle will return to its original waypoint trajectory, 1:Reset the origin of the waypoint to the present location + // @Bitmask: 0: Reset the origin of the waypoint to the present location + // @User: Standard + AP_GROUPINFO("OPTIONS", 5, AP_OAPathPlanner, _options, OA_OPTIONS_DEFAULT), +#endif + AP_GROUPEND }; diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index 318dcdc130..a7aae2a684 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.h @@ -54,6 +54,14 @@ public: OA_PATHPLAN_DIJKSTRA = 2 }; + // enumeration for _OPTION parameter + enum OARecoveryOptions { + OA_OPTION_DISABLED = 0, + OA_OPTION_WP_RESET = (1 << 0), + }; + + uint16_t get_options() const { return _options;} + static const struct AP_Param::GroupInfo var_info[]; private: @@ -81,10 +89,10 @@ private: } avoidance_result; // parameters - AP_Int8 _type; // avoidance algorith to be used + AP_Int8 _type; // avoidance algorithm to be used AP_Float _lookahead; // object avoidance will look this many meters ahead of vehicle AP_Float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle - + AP_Int16 _options; // Bitmask for options while recovering from Object Avoidance // internal variables used by front end HAL_Semaphore _rsem; // semaphore for multi-thread use of avoidance_request and avoidance_result bool _thread_created; // true once background thread has been created