Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -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 // 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) 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()); WITH_SEMAPHORE(AP::fence()->polyfence().get_loaded_fence_semaphore());
// avoidance is not required if no fences // avoidance is not required if no fences

11
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -205,6 +205,17 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
// avoidance thread that continually updates the avoidance_result structure based on avoidance_request // avoidance thread that continually updates the avoidance_result structure based on avoidance_request
void AP_OAPathPlanner::avoidance_thread() 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) { while (true) {
// if database queue needs attention, service it faster // if database queue needs attention, service it faster