AP_OAPathPlanner: Add param to reset WP origin while recovering from Bendy

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
 };
 

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