Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoidance: path planner avoids timeout when first activated

Browse Source
This commit is contained in:
Randy Mackay 2023-12-18 14:39:38 +09:00
parent 6fed0dbc7a
commit 82984577d2
2 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -199,7 +199,13 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
return OA_NOT_REQUIRED; return OA_NOT_REQUIRED;
} }
// check if just activated to avoid initial timeout error
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
if (now - _last_update_ms > 200) {
_activated_ms = now;
}
_last_update_ms = now;
WITH_SEMAPHORE(_rsem); WITH_SEMAPHORE(_rsem);
// place new request for the thread to work on // place new request for the thread to work on
@ -213,7 +219,7 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
const bool destination_matches = (destination.lat == avoidance_result.destination.lat) && (destination.lng == avoidance_result.destination.lng); const bool destination_matches = (destination.lat == avoidance_result.destination.lat) && (destination.lng == avoidance_result.destination.lng);
// check results have not timed out // check results have not timed out
const bool timed_out = now - avoidance_result.result_time_ms > OA_TIMEOUT_MS; const bool timed_out = (now - avoidance_result.result_time_ms > OA_TIMEOUT_MS) && (now - _activated_ms > OA_TIMEOUT_MS);
// return results from background thread's latest checks // return results from background thread's latest checks
if (destination_matches && !timed_out) { if (destination_matches && !timed_out) {

4
libraries/AC_Avoidance/AP_OAPathPlanner.h
View File

@ -115,7 +115,9 @@ private:
AP_OABendyRuler *_oabendyruler; // Bendy Ruler algorithm AP_OABendyRuler *_oabendyruler; // Bendy Ruler algorithm
AP_OADijkstra *_oadijkstra; // Dijkstra's algorithm AP_OADijkstra *_oadijkstra; // Dijkstra's algorithm
AP_OADatabase _oadatabase; // Database of dynamic objects to avoid AP_OADatabase _oadatabase; // Database of dynamic objects to avoid
uint32_t avoidance_latest_ms; // last time Dijkstra's or BendyRuler algorithms ran uint32_t avoidance_latest_ms; // last time Dijkstra's or BendyRuler algorithms ran (in the avoidance thread)
uint32_t _last_update_ms; // system time that mission_avoidance was called in main thread
uint32_t _activated_ms; // system time that object avoidance was most recently activated (used to avoid timeout error on first run)
bool proximity_only = true; bool proximity_only = true;
static AP_OAPathPlanner *_singleton; static AP_OAPathPlanner *_singleton;