AP_Logger: add fence message writer

libraries/AP_Logger/AP_Logger.cpp

@@ -891,6 +891,13 @@ void AP_Logger::Write_Rally()
 }
 #endif
 
+#if HAL_LOGGER_FENCE_ENABLED
+void AP_Logger::Write_Fence()
+{
+    FOR_EACH_BACKEND(Write_Fence());
+}
+#endif
+
 // output a FMT message for each backend if not already done so
 void AP_Logger::Safe_Write_Emit_FMT(log_write_fmt *f)
 {

libraries/AP_Logger/AP_Logger.h

@@ -61,6 +61,13 @@
 #include <AP_Logger/LogStructure.h>
 #include <AP_Vehicle/ModeReason.h>
 
+#include <AC_Fence/AC_Fence_config.h>
+#define HAL_LOGGER_FENCE_ENABLED (AP_FENCE_ENABLED && !defined(HAL_BUILD_AP_PERIPH))
+
+#if HAL_LOGGER_FENCE_ENABLED
+    #include <AC_Fence/AC_Fence.h>
+#endif
+
 #include <stdint.h>
 
 #include "LoggerMessageWriter.h"
@@ -294,6 +301,9 @@ public:
     void Write_RCOUT(void);
     void Write_RSSI();
     void Write_Rally();
+#if HAL_LOGGER_FENCE_ENABLED
+    void Write_Fence();
+#endif
     void Write_Power(void);
     void Write_Radio(const mavlink_radio_t &packet);
     void Write_Message(const char *message);

libraries/AP_Logger/AP_Logger_Backend.cpp

@@ -523,6 +523,32 @@ bool AP_Logger_Backend::Write_Rally()
 }
 #endif
 
+#if HAL_LOGGER_FENCE_ENABLED
+// Write a fence point
+bool AP_Logger_Backend::Write_FencePoint(uint8_t total, uint8_t sequence, const AC_PolyFenceItem &fence_point)
+{
+    const struct log_Fence pkt_fence{
+        LOG_PACKET_HEADER_INIT(LOG_FENCE_MSG),
+        time_us         : AP_HAL::micros64(),
+        total           : total,
+        sequence        : sequence,
+        type            : uint8_t(fence_point.type),
+        latitude        : fence_point.loc.x,
+        longitude       : fence_point.loc.y,
+        vertex_count    : fence_point.vertex_count,
+        radius          : fence_point.radius
+    };
+    return WriteBlock(&pkt_fence, sizeof(pkt_fence));
+}
+
+// Write all fence points
+bool AP_Logger_Backend::Write_Fence()
+{
+    // kick off asynchronous write:
+    return _startup_messagewriter->writeallfence();
+}
+#endif // HAL_LOGGER_FENCE_ENABLED
+
 
 bool AP_Logger_Backend::Write_VER()
 {

libraries/AP_Logger/AP_Logger_Backend.h

@@ -122,6 +122,10 @@ public:
                           uint8_t sequence,
                           const class RallyLocation &rally_point);
     bool Write_Rally();
+#if HAL_LOGGER_FENCE_ENABLED
+    bool Write_FencePoint(uint8_t total, uint8_t sequence, const AC_PolyFenceItem &fence_point);
+    bool Write_Fence();
+#endif
     bool Write_Format(const struct LogStructure *structure);
     bool Write_Message(const char *message);
     bool Write_MessageF(const char *fmt, ...);

libraries/AP_Logger/LogStructure.h

@@ -139,6 +139,7 @@ const struct MultiplierStructure log_Multipliers[] = {
 #include <AP_AIS/LogStructure.h>
 #include <AP_HAL_ChibiOS/LogStructure.h>
 #include <AP_RPM/LogStructure.h>
+#include <AC_Fence/LogStructure.h>
 
 // structure used to define logging format
 // It is packed on ChibiOS to save flash space; however, this causes problems
@@ -1327,7 +1328,8 @@ LOG_STRUCTURE_FROM_AIS \
     { LOG_VER_MSG, sizeof(log_VER), \
       "VER",   "QBHBBBBIZH", "TimeUS,BT,BST,Maj,Min,Pat,FWT,GH,FWS,APJ", "s---------", "F---------", false }, \
     { LOG_MOTBATT_MSG, sizeof(log_MotBatt), \
-      "MOTB", "QffffB",  "TimeUS,LiftMax,BatVolt,ThLimit,ThrAvMx,FailFlags", "s-----", "F-----" , true }
+      "MOTB", "QffffB",  "TimeUS,LiftMax,BatVolt,ThLimit,ThrAvMx,FailFlags", "s-----", "F-----" , true }, \
+LOG_STRUCTURE_FROM_FENCE
 
 // message types 0 to 63 reserved for vehicle specific use
 
@@ -1412,6 +1414,7 @@ enum LogMessages : uint8_t {
     LOG_VER_MSG,
     LOG_RCOUT2_MSG,
     LOG_RCOUT3_MSG,
+    LOG_IDS_FROM_FENCE,
 
     _LOG_LAST_MSG_
 };

libraries/AP_Logger/LoggerMessageWriter.cpp

@@ -44,6 +44,9 @@ void LoggerMessageWriter_DFLogStart::reset()
 #if HAL_RALLY_ENABLED
     _writeallrallypoints.reset();
 #endif
+#if HAL_LOGGER_FENCE_ENABLED
+    _writeallpolyfence.reset();
+#endif
 
     stage = Stage::FORMATS;
     next_format_to_send = 0;
@@ -151,6 +154,14 @@ void LoggerMessageWriter_DFLogStart::process()
                 return;
             }
         }
+#endif
+#if HAL_LOGGER_FENCE_ENABLED
+        if (!_writeallpolyfence.finished()) {
+            _writeallpolyfence.process();
+            if (!_writeallpolyfence.finished()) {
+                return;
+            }
+        }
 #endif
         stage = Stage::VEHICLE_MESSAGES;
         FALLTHROUGH;
@@ -201,6 +212,19 @@ bool LoggerMessageWriter_DFLogStart::writeallrallypoints()
 }
 #endif
 
+#if HAL_LOGGER_FENCE_ENABLED
+bool LoggerMessageWriter_DFLogStart::writeallfence()
+{
+    if (stage != Stage::DONE) {
+        return false;
+    }
+    stage = Stage::RUNNING_SUBWRITERS;
+    _finished = false;
+    _writeallpolyfence.reset();
+    return true;
+}
+#endif
+
 void LoggerMessageWriter_WriteSysInfo::reset()
 {
     LoggerMessageWriter::reset();
@@ -382,3 +406,61 @@ void LoggerMessageWriter_WriteEntireMission::reset()
     stage = Stage::WRITE_NEW_MISSION_MESSAGE;
     _mission_number_to_send = 0;
 }
+
+#if HAL_LOGGER_FENCE_ENABLED
+#if APM_BUILD_TYPE(APM_BUILD_Replay)
+// dummy methods to allow build with Replay
+void LoggerMessageWriter_Write_Polyfence::process() { };
+void LoggerMessageWriter_Write_Polyfence::reset() { };
+#else
+void LoggerMessageWriter_Write_Polyfence::process() {
+
+    AC_Fence *fence = AP::fence();
+    if (fence == nullptr) {
+        return;
+    }
+
+    switch(stage) {
+
+    case Stage::WRITE_NEW_FENCE_MESSAGE:
+        if (!_logger_backend->Write_Message("New fence")) {
+            return; // call me again
+        }
+        stage = Stage::WRITE_FENCE_ITEMS;
+        FALLTHROUGH;
+
+    case Stage::WRITE_FENCE_ITEMS: {
+        while (_fence_number_to_send < fence->polyfence().num_stored_items()) {
+            if (out_of_time_for_writing_messages()) {
+                return;
+            }
+
+            // upon failure to write the fence we will re-read from
+            // storage; this could be improved.
+            AC_PolyFenceItem fenceitem {};
+            if (fence->polyfence().get_item(_fence_number_to_send, fenceitem)) {
+                if (!_logger_backend->Write_FencePoint(fence->polyfence().num_stored_items(), _fence_number_to_send, fenceitem)) {
+                    return; // call me again
+                }
+            }
+            _fence_number_to_send++;
+        }
+        stage = Stage::DONE;
+        FALLTHROUGH;
+    }
+
+    case Stage::DONE:
+        break;
+    }
+
+    _finished = true;
+}
+
+void LoggerMessageWriter_Write_Polyfence::reset()
+{
+    LoggerMessageWriter::reset();
+    stage = Stage::WRITE_NEW_FENCE_MESSAGE;
+    _fence_number_to_send = 0;
+}
+#endif // !APM_BUILD_TYPE(APM_BUILD_Replay)
+#endif // AP_FENCE_ENABLED

libraries/AP_Logger/LoggerMessageWriter.h

@@ -74,6 +74,25 @@ private:
     Stage stage = Stage::WRITE_NEW_RALLY_MESSAGE;
 };
 
+#if HAL_LOGGER_FENCE_ENABLED
+class LoggerMessageWriter_Write_Polyfence : public LoggerMessageWriter {
+public:
+
+    void reset() override;
+    void process() override;
+
+private:
+    enum Stage {
+        WRITE_NEW_FENCE_MESSAGE = 0,
+        WRITE_FENCE_ITEMS,
+        DONE
+    };
+
+    uint16_t _fence_number_to_send;
+    Stage stage;
+};
+#endif // HAL_LOGGER_FENCE_ENABLED
+
 class LoggerMessageWriter_DFLogStart : public LoggerMessageWriter {
 public:
     LoggerMessageWriter_DFLogStart() :
@@ -83,6 +102,9 @@ public:
 #endif
 #if HAL_RALLY_ENABLED
         , _writeallrallypoints()
+#endif
+#if HAL_LOGGER_FENCE_ENABLED
+        , _writeallpolyfence()
 #endif
         {
         }
@@ -95,6 +117,9 @@ public:
 #endif
 #if HAL_RALLY_ENABLED
         _writeallrallypoints.set_logger_backend(backend);
+#endif
+#if HAL_LOGGER_FENCE_ENABLED
+        _writeallpolyfence.set_logger_backend(backend);
 #endif
     }
 
@@ -113,6 +138,9 @@ public:
 #if HAL_RALLY_ENABLED
     bool writeallrallypoints();
 #endif
+#if HAL_LOGGER_FENCE_ENABLED
+    bool writeallfence();
+#endif
 
 private:
 
@@ -151,4 +179,7 @@ private:
 #if HAL_RALLY_ENABLED
     LoggerMessageWriter_WriteAllRallyPoints _writeallrallypoints;
 #endif
+#if HAL_LOGGER_FENCE_ENABLED
+    LoggerMessageWriter_Write_Polyfence _writeallpolyfence;
+#endif
 };