AP_Logger: add Write_OA

libraries/AP_Logger/AP_Logger.h

@@ -274,6 +274,7 @@ public:
     void Write_Beacon(AP_Beacon &beacon);
     void Write_Proximity(AP_Proximity &proximity);
     void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);
+    void Write_OA(uint8_t algorithm, const Location& final_dest, const Location& oa_dest);
 
     void Write(const char *name, const char *labels, const char *fmt, ...);
     void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

libraries/AP_Logger/LogFile.cpp

@@ -1694,3 +1694,17 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points
     };
     WriteBlock(&pkt_srtl, sizeof(pkt_srtl));
 }
+
+void AP_Logger::Write_OA(uint8_t algorithm, const Location& final_dest, const Location& oa_dest)
+{
+    struct log_OA pkt = {
+        LOG_PACKET_HEADER_INIT(LOG_OA_MSG),
+        time_us     : AP_HAL::micros64(),
+        algorithm   : algorithm,
+        final_lat   : final_dest.lat,
+        final_lng   : final_dest.lng,
+        oa_lat      : oa_dest.lat,
+        oa_lng      : oa_dest.lng,
+    };
+    WriteBlock(&pkt, sizeof(pkt));
+}

libraries/AP_Logger/LogStructure.h

@@ -1125,6 +1125,16 @@ struct PACKED log_SRTL {
     float D;
 };
 
+struct PACKED log_OA {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t algorithm;
+    int32_t final_lat;
+    int32_t final_lng;
+    int32_t oa_lat;
+    int32_t oa_lng;
+};
+
 struct PACKED log_DSTL {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -1337,7 +1347,9 @@ struct PACKED log_Arm_Disarm {
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance),                     \
       "PM",  "QHHIIHIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,IntErr,SPICnt,I2CCnt", "s---b%--", "F---0A--" }, \
     { LOG_SRTL_MSG, sizeof(log_SRTL), \
-      "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }
+      "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \
+    { LOG_OA_MSG, sizeof(log_OA), \
+      "OA","QBLLLL","TimeUS,Algo,DLat,DLng,OALat,OALng", "s-----", "F-GGGG" }
 
 // messages for more advanced boards
 #define LOG_EXTRA_STRUCTURES \
@@ -1681,6 +1693,7 @@ enum LogMessages : uint8_t {
     LOG_ERROR_MSG,
     LOG_ADSB_MSG,
     LOG_ARM_DISARM_MSG,
+    LOG_OA_MSG,
 
     _LOG_LAST_MSG_
 };