AP_OAPathPlanner: report errors to caller

libraries/AC_Avoidance/AP_OABendyRuler.h

@@ -22,6 +22,7 @@ public:
     void set_config(float lookahead, float margin_max) { _lookahead = MAX(lookahead, 1.0f); _margin_max = MAX(margin_max, 0.0f); }
 
     // run background task to find best path and update avoidance_results
+    // returns true and populates origin_new and destination_new if OA is required.  returns false if OA is not required
     bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new);
 
 private:

libraries/AC_Avoidance/AP_OADijkstra.cpp

@@ -28,18 +28,18 @@ AP_OADijkstra::AP_OADijkstra() :
 }
 
 // calculate a destination to avoid the polygon fence
-// returns true if avoidance is required and updates origin_new and destination_new
+// returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required
-bool AP_OADijkstra::update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new)
+AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new)
 {
     // require ekf origin to have been set
     struct Location ekf_origin {};
     if (!AP::ahrs().get_origin(ekf_origin)) {
-        return false;
+        return DIJKSTRA_STATE_NOT_REQUIRED;
     }
 
     // no avoidance required if fence is disabled
     if (!polygon_fence_enabled()) {
-        return false;
+        return DIJKSTRA_STATE_NOT_REQUIRED;
     }
 
     // check for fence updates
@@ -53,7 +53,7 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
         if (!_polyfence_with_margin_ok) {
             _polyfence_visgraph_ok = false;
             _shortest_path_ok = false;
-            return false;
+            return DIJKSTRA_STATE_ERROR;
         }
     }
 
@@ -62,7 +62,7 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
         _polyfence_visgraph_ok = create_polygon_fence_visgraph();
         if (!_polyfence_visgraph_ok) {
             _shortest_path_ok = false;
-            return false;
+            return DIJKSTRA_STATE_ERROR;
         }
     }
 
@@ -76,7 +76,7 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
     if (!_shortest_path_ok) {
         _shortest_path_ok = calc_shortest_path(current_loc, destination);
         if (!_shortest_path_ok) {
-            return false;
+            return DIJKSTRA_STATE_ERROR;
         }
         // start from 2nd point on path (first is the original origin)
         _path_idx_returned = 1;
@@ -109,10 +109,10 @@ bool AP_OADijkstra::update(const Location &current_loc, const Location &destinat
         if (near_oa_wp || past_oa_wp) {
             _path_idx_returned++;
         }
-        return true;
+        return DIJKSTRA_STATE_SUCCESS;
     }
 
-    return false;
+    return DIJKSTRA_STATE_ERROR;
 }
 
 // returns true if polygon fence is enabled

libraries/AC_Avoidance/AP_OADijkstra.h

@@ -22,9 +22,16 @@ public:
     // set fence margin (in meters) used when creating "safe positions" within the polygon fence
     void set_fence_margin(float margin) { _polyfence_margin = MAX(margin, 0.0f); }
 
+    // update return status enum
+    enum AP_OADijkstra_State : uint8_t {
+        DIJKSTRA_STATE_NOT_REQUIRED = 0,
+        DIJKSTRA_STATE_ERROR,
+        DIJKSTRA_STATE_SUCCESS
+    };
+
     // calculate a destination to avoid the polygon fence
-    // returns true if avoidance is required and target destination is placed in result_loc
+    // returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required
-    bool update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new);
+    AP_OADijkstra_State update(const Location &current_loc, const Location &destination, Location& origin_new, Location& destination_new);
 
 private:
 

libraries/AC_Avoidance/AP_OAPathPlanner.cpp

@@ -114,7 +114,7 @@ bool AP_OAPathPlanner::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
 
 // provides an alternative target location if path planning around obstacles is required
 // returns true and updates result_loc with an intermediate location
-bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
+AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
                                          const Location &origin,
                                          const Location &destination,
                                          Location &result_origin,
@@ -122,7 +122,7 @@ bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
 {
     // exit immediately if disabled
     if (_type == OA_PATHPLAN_DISABLED) {
-        return false;
+        return OA_NOT_REQUIRED;
     }
 
     WITH_SEMAPHORE(_rsem);
@@ -134,7 +134,7 @@ bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
         if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OAPathPlanner::avoidance_thread, void),
                                           "avoidance",
                                           8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
-            return false;
+            return OA_ERROR;
         }
         _thread_created = true;
     }
@@ -148,10 +148,14 @@ bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
     avoidance_request.ground_speed_vec = AP::ahrs().groundspeed_vector();
     avoidance_request.request_time_ms = now;
 
+    // check result's destination matches our request
+    const bool destination_matches = (destination.lat == avoidance_result.destination.lat) && (destination.lng == avoidance_result.destination.lng);
+
+    // check results have not timed out
+    const bool timed_out = now - avoidance_result.result_time_ms > OA_TIMEOUT_MS;
+
     // return results from background thread's latest checks
-    if (destination.lat == avoidance_result.destination.lat &&
+    if (destination_matches && !timed_out) {
-        destination.lng == avoidance_result.destination.lng &&
-        now - avoidance_result.result_time_ms < OA_TIMEOUT_MS) {
         // we have a result from the thread
         result_origin = avoidance_result.origin_new;
         result_destination = avoidance_result.destination_new;
@@ -160,12 +164,16 @@ bool AP_OAPathPlanner::mission_avoidance(const Location &current_loc,
             _logged_time_ms = avoidance_result.result_time_ms;
             AP::logger().Write_OA(_type, destination, result_destination);
         }
-        return avoidance_result.avoidance_needed;
+        return avoidance_result.ret_state;
     }
 
-    // do not performance avoidance because background thread's results were
+    // if timeout then path planner is taking too long to respond
-    // run against a different destination or they are simply not required
+    if (timed_out) {
-    return false;
+        return OA_ERROR;
+    }
+
+    // background thread is working on a new destination
+    return OA_PROCESSING;
 }
 
 // avoidance thread that continually updates the avoidance_result structure based on avoidance_request
@@ -195,7 +203,7 @@ void AP_OAPathPlanner::avoidance_thread()
         }
 
         // run background task looking for best alternative destination
-        bool res = false;
+        OA_RetState res = OA_NOT_REQUIRED;
         switch (_type) {
         case OA_PATHPLAN_DISABLED:
             continue;
@@ -204,14 +212,27 @@ void AP_OAPathPlanner::avoidance_thread()
                 continue;
             }
             _oabendyruler->set_config(_lookahead, _margin_max);
-            res = _oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new);
+            if (_oabendyruler->update(avoidance_request2.current_loc, avoidance_request2.destination, avoidance_request2.ground_speed_vec, origin_new, destination_new)) {
+                res = OA_SUCCESS;
+            }
             break;
         case OA_PATHPLAN_DIJKSTRA:
             if (_oadijkstra == nullptr) {
                 continue;
             }
             _oadijkstra->set_fence_margin(_margin_max);
-            res = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new);
+            const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new);
+            switch (dijkstra_state) {
+            case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED:
+                res = OA_NOT_REQUIRED;
+                break;
+            case AP_OADijkstra::DIJKSTRA_STATE_ERROR:
+                res = OA_ERROR;
+                break;
+            case AP_OADijkstra::DIJKSTRA_STATE_SUCCESS:
+                res = OA_SUCCESS;
+                break;
+            }
             break;
         }
 
@@ -222,7 +243,7 @@ void AP_OAPathPlanner::avoidance_thread()
             avoidance_result.origin_new = res ? origin_new : avoidance_result.origin_new;
             avoidance_result.destination_new = res ? destination_new : avoidance_result.destination;
             avoidance_result.result_time_ms = AP_HAL::millis();
-            avoidance_result.avoidance_needed = res;
+            avoidance_result.ret_state = res;
         }
     }
 }

libraries/AC_Avoidance/AP_OAPathPlanner.h

@@ -30,9 +30,17 @@ public:
     /// returns true if all pre-takeoff checks have completed successfully
     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
 
+    // object avoidance processing return status enum
+    enum OA_RetState : uint8_t {
+        OA_NOT_REQUIRED = 0,            // object avoidance is not required
+        OA_PROCESSING,                  // still calculating alternative path
+        OA_ERROR,                       // error during calculation
+        OA_SUCCESS                      // success
+    };
+
     // provides an alternative target location if path planning around obstacles is required
     // returns true and updates result_origin and result_destination with an intermediate path
-    bool mission_avoidance(const Location &current_loc,
+    OA_RetState mission_avoidance(const Location &current_loc,
                            const Location &origin,
                            const Location &destination,
                            Location &result_origin,
@@ -67,7 +75,7 @@ private:
         Location origin_new;        // intermediate origin.  The start of line segment that vehicle should follow
         Location destination_new;   // intermediate destination vehicle should move towards
         uint32_t result_time_ms;    // system time the result was calculated (used to verify the result is recent)
-        bool avoidance_needed;      // true if the vehicle should move along the path from origin_new to destination_new
+        OA_RetState ret_state;      // OA_SUCCESS if the vehicle should move along the path from origin_new to destination_new
     } avoidance_result;
 
     // parameters