Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: Check if origin is set before any obstacle avoidance algorithm is run

Browse Source
This commit is contained in:
Rishabh 2020-06-16 16:47:16 +05:30 committed by Randy Mackay
parent 8d2f2443a2
commit 34b17d0993
2 changed files with 12 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -37,16 +37,6 @@ AP_OADijkstra::AP_OADijkstra() :
// 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 &current_loc, const Location &destination, Location& origin_new, Location& destination_new)
{
// require ekf origin to have been set
struct Location ekf_origin {};
{
WITH_SEMAPHORE(AP::ahrs().get_semaphore());
if (!AP::ahrs().get_origin(ekf_origin)) {
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}
}
WITH_SEMAPHORE(AP::fence()->polyfence().get_loaded_fence_semaphore());
// avoidance is not required if no fences

13
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -204,7 +204,18 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
// avoidance thread that continually updates the avoidance_result structure based on avoidance_request
void AP_OAPathPlanner::avoidance_thread()
{
{
// require ekf origin to have been set
bool origin_set = false;
while (!origin_set) {
hal.scheduler->delay(500);
struct Location ekf_origin {};
{
WITH_SEMAPHORE(AP::ahrs().get_semaphore());
origin_set = AP::ahrs().get_origin(ekf_origin);
}
}
while (true) {
// if database queue needs attention, service it faster