From ae9300134a41dc9c07645a6596218e46dfa4fb26 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Mon, 11 Oct 2021 23:35:36 +0100 Subject: [PATCH] AC_Avoidance: add option to log vis graph --- .../AC_Avoidance/AC_Avoidance_Logging.cpp | 14 +++++++++++ libraries/AC_Avoidance/AP_OADijkstra.cpp | 17 +++++++++++++- libraries/AC_Avoidance/AP_OADijkstra.h | 10 ++++++-- libraries/AC_Avoidance/AP_OAPathPlanner.cpp | 11 +++++---- libraries/AC_Avoidance/AP_OAPathPlanner.h | 1 + libraries/AC_Avoidance/LogStructure.h | 23 +++++++++++++++++-- 6 files changed, 66 insertions(+), 10 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp b/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp index a904f4cd6e..f02ad8a2fd 100644 --- a/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp +++ b/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp @@ -46,6 +46,20 @@ void AP_OADijkstra::Write_OADijkstra(const uint8_t state, const uint8_t error_id AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +void AP_OADijkstra::Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const +{ + const struct log_OD_Visgraph pkt{ + LOG_PACKET_HEADER_INIT(LOG_OD_VISGRAPH_MSG), + time_us : AP_HAL::micros64(), + version : version, + point_num : point_num, + Lat : Lat, + Lon : Lon, + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + + void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const { const struct log_SimpleAvoid pkt{ diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index 3dac088bdc..df374f34b6 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -14,6 +14,7 @@ */ #include "AP_OADijkstra.h" +#include "AP_OAPathPlanner.h" #include #include @@ -24,7 +25,8 @@ #define OA_DIJKSTRA_ERROR_REPORTING_INTERVAL_MS 5000 // failure messages sent to GCS every 5 seconds /// Constructor -AP_OADijkstra::AP_OADijkstra() : +AP_OADijkstra::AP_OADijkstra(AP_Int16 &options) : + _options(options), _inclusion_polygon_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK), _exclusion_polygon_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK), _exclusion_circle_pts(OA_DIJKSTRA_EXPANDING_ARRAY_ELEMENTS_PER_CHUNK), @@ -112,6 +114,19 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; } + // reset logging count to restart logging updated graph + _log_num_points = 0; + _log_visgraph_version++; + } + + // Log one visgraph point per loop + if (_polyfence_visgraph_ok && (_log_num_points < total_numpoints()) && (_options & AP_OAPathPlanner::OARecoveryOptions::OA_OPTION_LOG_DIJKSTRA_POINTS) ) { + Vector2f vis_point; + if (get_point(_log_num_points, vis_point)) { + Location log_location(Vector3f(vis_point.x, vis_point.y, 0.0f), Location::AltFrame::ABOVE_ORIGIN); + Write_Visgraph_point(_log_visgraph_version, _log_num_points, log_location.lat, log_location.lng); + _log_num_points++; + } } // rebuild path if destination has changed diff --git a/libraries/AC_Avoidance/AP_OADijkstra.h b/libraries/AC_Avoidance/AP_OADijkstra.h index 7d5f7b2aa7..baccdc536e 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/libraries/AC_Avoidance/AP_OADijkstra.h @@ -13,7 +13,7 @@ class AP_OADijkstra { public: - AP_OADijkstra(); + AP_OADijkstra(AP_Int16 &options); /* Do not allow copies */ AP_OADijkstra(const AP_OADijkstra &other) = delete; @@ -190,6 +190,12 @@ private: AP_OADijkstra_Error _error_last_id; // last error id sent to GCS uint32_t _error_last_report_ms; // last time an error message was sent to GCS - // Logging function + // Logging functions void Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const; + void Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const; + uint8_t _log_num_points; + uint8_t _log_visgraph_version; + + // refernce to AP_OAPathPlanner options param + AP_Int16 &_options; }; diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 836471c447..84a9d47d4d 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -55,12 +55,13 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = { // @Path: AP_OADatabase.cpp AP_SUBGROUPINFO(_oadatabase, "DB_", 4, AP_OAPathPlanner, AP_OADatabase), - // @Param{Rover}: OPTIONS + // @Param: OPTIONS // @DisplayName: Options while recovering from Object Avoidance // @Description: Bitmask which will govern vehicles behaviour while recovering from Obstacle Avoidance (i.e Avoidance is turned off after the path ahead is clear). - // @Bitmask: 0: Reset the origin of the waypoint to the present location + // @Bitmask{Rover}: 0: Reset the origin of the waypoint to the present location, 1: log Dijkstra points + // @Bitmask{Copter}: 1: log Dijkstra points // @User: Standard - AP_GROUPINFO_FRAME("OPTIONS", 5, AP_OAPathPlanner, _options, OA_OPTIONS_DEFAULT, AP_PARAM_FRAME_ROVER), + AP_GROUPINFO("OPTIONS", 5, AP_OAPathPlanner, _options, OA_OPTIONS_DEFAULT), // @Group: BR_ // @Path: AP_OABendyRuler.cpp @@ -93,12 +94,12 @@ void AP_OAPathPlanner::init() break; case OA_PATHPLAN_DIJKSTRA: if (_oadijkstra == nullptr) { - _oadijkstra = new AP_OADijkstra(); + _oadijkstra = new AP_OADijkstra(_options); } break; case OA_PATHPLAN_DJIKSTRA_BENDYRULER: if (_oadijkstra == nullptr) { - _oadijkstra = new AP_OADijkstra(); + _oadijkstra = new AP_OADijkstra(_options); } if (_oabendyruler == nullptr) { _oabendyruler = new AP_OABendyRuler(); diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index e064712a89..4b12a58f78 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.h @@ -69,6 +69,7 @@ public: enum OARecoveryOptions { OA_OPTION_DISABLED = 0, OA_OPTION_WP_RESET = (1 << 0), + OA_OPTION_LOG_DIJKSTRA_POINTS = (1 << 1), }; uint16_t get_options() const { return _options;} diff --git a/libraries/AC_Avoidance/LogStructure.h b/libraries/AC_Avoidance/LogStructure.h index cc62df9007..585eca8251 100644 --- a/libraries/AC_Avoidance/LogStructure.h +++ b/libraries/AC_Avoidance/LogStructure.h @@ -5,7 +5,8 @@ #define LOG_IDS_FROM_AVOIDANCE \ LOG_OA_BENDYRULER_MSG, \ LOG_OA_DIJKSTRA_MSG, \ - LOG_SIMPLE_AVOID_MSG + LOG_SIMPLE_AVOID_MSG, \ + LOG_OD_VISGRAPH_MSG // @LoggerMessage: OABR // @Description: Object avoidance (Bendy Ruler) diagnostics @@ -89,10 +90,28 @@ struct PACKED log_SimpleAvoid { uint8_t backing_up; }; +// @LoggerMessage: OAVG +// @Description: Object avoidance path planning visgraph points +// @Field: TimeUS: Time since system startup +// @Field: version: Visgraph version, increments each time the visgraph is re-generated +// @Field: point_num: point number in visgraph +// @Field: Lat: Latitude +// @Field: Lon: longitude +struct PACKED log_OD_Visgraph { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t version; + uint8_t point_num; + int32_t Lat; + int32_t Lon; +}; + #define LOG_STRUCTURE_FROM_AVOIDANCE \ { LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \ "OABR","QBBHHHBfLLiLLi","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s-bddd-mDUmDUm", "F-------GGBGGB" , true }, \ { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \ "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" , true }, \ { LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \ - "SA", "QBffffffB","TimeUS,State,DVelX,DVelY,DVelZ,MVelX,MVelY,MVelZ,Back", "sbnnnnnnb", "F--------", true }, + "SA", "QBffffffB","TimeUS,State,DVelX,DVelY,DVelZ,MVelX,MVelY,MVelZ,Back", "sbnnnnnnb", "F--------", true }, \ + { LOG_OD_VISGRAPH_MSG, sizeof(log_OD_Visgraph), \ + "OAVG", "QBBLL", "TimeUS,version,point_num,Lat,Lon", "s--DU", "F--GG", true},