AC_Avoidance: add option to log vis graph

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{

libraries/AC_Avoidance/AP_OADijkstra.cpp

@@ -14,6 +14,7 @@
  */
 
 #include "AP_OADijkstra.h"
+#include "AP_OAPathPlanner.h"
 
 #include <AC_Fence/AC_Fence.h>
 #include <AP_AHRS/AP_AHRS.h>
@@ -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 &current
             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

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;
 };

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();

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;}

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},