aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_Avoidance: replace Write_OA with Write_OABendyRuler and Write_OADijkstra

Browse Source
This commit is contained in:
Randy Mackay 2019-08-05 15:23:17 +09:00
parent b666b172c4
commit 488d1ab60a
4 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_Avoidance/AP_OABendyRuler.cpp
View File

@ -17,6 +17,7 @@
#include <AC_Avoidance/AP_OADatabase.h>
#include <AC_Fence/AC_Fence.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h>
const int16_t OA_BENDYRULER_BEARING_INC = 5; // check every 5 degrees around vehicle
const float OA_BENDYRULER_LOOKAHEAD_STEP2_RATIO = 1.0f; // step2's lookahead length as a ratio of step1's lookahead length
@ -116,7 +117,9 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
destination_new.offset_bearing(bearing_test, distance_to_dest);
_current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f);
// if the chosen direction is directly towards the destination turn off avoidance
return (i != 0 || j != 0);
const bool active = (i != 0 || j != 0);
AP::logger().Write_OABendyRuler(active, bearing_to_dest, margin, destination, destination_new);
return active;
}
}
}
@ -140,6 +143,9 @@ bool AP_OABendyRuler::update(const Location& current_loc, const Location& destin
destination_new = current_loc;
destination_new.offset_bearing(chosen_bearing, distance_to_dest);
// log results
AP::logger().Write_OABendyRuler(true, chosen_bearing, best_margin, destination, destination_new);
return true;
}

9
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -17,6 +17,7 @@
#include <AC_Fence/AC_Fence.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h>
#define OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK 32 // expanding arrays for inner polygon fence and paths to destination will grow in increments of 20 elements
#define OA_DIJKSTRA_POLYGON_SHORTPATH_NOTSET_IDX 255 // index use to indicate we do not have a tentative short path for a node
@ -36,11 +37,13 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
// require ekf origin to have been set
struct Location ekf_origin {};
if (!AP::ahrs().get_origin(ekf_origin)) {
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}
// no avoidance required if fence is disabled
if (!polygon_fence_enabled()) {
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, destination, destination);
return DIJKSTRA_STATE_NOT_REQUIRED;
}
@ -55,6 +58,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
if (!_polyfence_with_margin_ok) {
_polyfence_with_margin_ok = create_polygon_fence_with_margin(_polyfence_margin * 100.0f);
if (!_polyfence_with_margin_ok) {
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}
}
@ -64,6 +68,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
_polyfence_visgraph_ok = create_polygon_fence_visgraph();
if (!_polyfence_visgraph_ok) {
_shortest_path_ok = false;
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}
}
@ -111,9 +116,13 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location &current
if (near_oa_wp || past_oa_wp) {
_path_idx_returned++;
}
// log success
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, _path_idx_returned, _path_numpoints, destination, destination_new);
return DIJKSTRA_STATE_SUCCESS;
}
// log failure
AP::logger().Write_OADijkstra(DIJKSTRA_STATE_ERROR, 0, 0, destination, destination);
return DIJKSTRA_STATE_ERROR;
}

5
libraries/AC_Avoidance/AP_OAPathPlanner.cpp
View File

@ -178,11 +178,6 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location
// we have a result from the thread
result_origin = avoidance_result.origin_new;
result_destination = avoidance_result.destination_new;
// log result
if (avoidance_result.result_time_ms != _logged_time_ms) {
_logged_time_ms = avoidance_result.result_time_ms;
AP::logger().Write_OA(_type, destination, result_destination);
}
return avoidance_result.ret_state;
}

1
libraries/AC_Avoidance/AP_OAPathPlanner.h
View File

@ -91,7 +91,6 @@ private:
AP_OABendyRuler *_oabendyruler; // Bendy Ruler algorithm
AP_OADijkstra *_oadijkstra; // Dijkstra's algorithm
AP_OADatabase _oadatabase; // Database of dynamic objects to avoid
uint32_t _logged_time_ms; // result_time_ms of last result logged (triggers logging of new results)
#if !HAL_MINIMIZE_FEATURES
uint32_t avoidance_latest_ms; // last time Dijkstra's or BendyRuler algorithms ran
#endif