diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.h b/libraries/AC_Avoidance/AP_OABendyRuler.h index 958cd56140..312d5d5cb5 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.h +++ b/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: diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index b9e38f4e56..eccfa882b7 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/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 -bool AP_OADijkstra::update(const Location ¤t_loc, const Location &destination, Location& origin_new, Location& destination_new) +// returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required +AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t_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 ¤t_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 ¤t_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 ¤t_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 ¤t_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 diff --git a/libraries/AC_Avoidance/AP_OADijkstra.h b/libraries/AC_Avoidance/AP_OADijkstra.h index 1efc615140..5dc3a89726 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/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 - bool update(const Location ¤t_loc, const Location &destination, Location& origin_new, Location& destination_new); + // returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required + AP_OADijkstra_State update(const Location ¤t_loc, const Location &destination, Location& origin_new, Location& destination_new); private: diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 311aa72f90..0abceb9a35 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/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 ¤t_loc, +AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location ¤t_loc, const Location &origin, const Location &destination, Location &result_origin, @@ -122,7 +122,7 @@ bool AP_OAPathPlanner::mission_avoidance(const Location ¤t_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 ¤t_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 ¤t_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 && - destination.lng == avoidance_result.destination.lng && - now - avoidance_result.result_time_ms < OA_TIMEOUT_MS) { + if (destination_matches && !timed_out) { // 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 ¤t_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 - // run against a different destination or they are simply not required - return false; + // if timeout then path planner is taking too long to respond + if (timed_out) { + 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; } } } diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index 30631cba69..d066308689 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/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 ¤t_loc, + OA_RetState mission_avoidance(const Location ¤t_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