From 488d1ab60af011ef83971318acaa682eee7c4ff0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 5 Aug 2019 15:23:17 +0900 Subject: [PATCH] AC_Avoidance: replace Write_OA with Write_OABendyRuler and Write_OADijkstra --- libraries/AC_Avoidance/AP_OABendyRuler.cpp | 8 +++++++- libraries/AC_Avoidance/AP_OADijkstra.cpp | 9 +++++++++ libraries/AC_Avoidance/AP_OAPathPlanner.cpp | 5 ----- libraries/AC_Avoidance/AP_OAPathPlanner.h | 1 - 4 files changed, 16 insertions(+), 7 deletions(-) diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index 68234b5dc1..cc429b84f5 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -17,6 +17,7 @@ #include #include #include +#include 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; } diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index 806f72d040..db80f11ab7 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -17,6 +17,7 @@ #include #include +#include #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 ¤t // 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 ¤t 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 ¤t _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 ¤t 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; } diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index f28fcfffdc..d3bc5d7d01 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -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; } diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index 0ee008954b..1c765573df 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.h @@ -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