From 85b4ee289e8386deba1203aa52e9acd9d84c6574 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 2 Feb 2022 11:46:06 +1100
Subject: [PATCH] AP_Logger: log VER - ArduPilot version information

---
 libraries/AP_Logger/AP_Logger_Backend.cpp   | 26 +++++++++++++++++++++
 libraries/AP_Logger/AP_Logger_Backend.h     |  1 +
 libraries/AP_Logger/LogStructure.h          | 18 ++++++++++++++
 libraries/AP_Logger/LoggerMessageWriter.cpp |  9 ++++++-
 libraries/AP_Logger/LoggerMessageWriter.h   |  1 +
 5 files changed, 54 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp
index a42e4b8965..b1a993164d 100644
--- a/libraries/AP_Logger/AP_Logger_Backend.cpp
+++ b/libraries/AP_Logger/AP_Logger_Backend.cpp
@@ -2,6 +2,7 @@
 
 #include "LoggerMessageWriter.h"
 
+#include "AP_Common/AP_FWVersion.h"
 #include <AP_InternalError/AP_InternalError.h>
 #include <AP_Scheduler/AP_Scheduler.h>
 
@@ -519,6 +520,31 @@ bool AP_Logger_Backend::Write_Rally()
 }
 #endif
 
+
+bool AP_Logger_Backend::Write_VER()
+{
+    const AP_FWVersion &fwver = AP::fwversion();
+
+    log_VER pkt{
+        LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
+        time_us  : AP_HAL::micros64(),
+        board_type : fwver.board_type,
+        board_subtype: fwver.board_subtype,
+        major: fwver.major,
+        minor: fwver.minor,
+        patch: fwver.patch,
+        fw_type: fwver.fw_type,
+        git_hash: fwver.fw_hash,
+    };
+    strncpy(pkt.fw_string, fwver.fw_string, ARRAY_SIZE(pkt.fw_string)-1);
+
+#ifdef APJ_BOARD_ID
+    pkt._APJ_BOARD_ID = APJ_BOARD_ID;
+#endif
+
+    return WriteCriticalBlock(&pkt, sizeof(pkt));
+}
+
 /*
   convert a list entry number back into a log number (which can then
   be converted into a filename).  A "list entry number" is a sequence
diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h
index 0053381403..58d77c46c4 100644
--- a/libraries/AP_Logger/AP_Logger_Backend.h
+++ b/libraries/AP_Logger/AP_Logger_Backend.h
@@ -130,6 +130,7 @@ public:
     bool Write_Parameter(const AP_Param *ap,
                              const AP_Param::ParamToken &token,
                              enum ap_var_type type);
+    bool Write_VER();
 
     uint32_t num_dropped(void) const {
         return _dropped;
diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
index 59441de1b8..2dc16e84ce 100644
--- a/libraries/AP_Logger/LogStructure.h
+++ b/libraries/AP_Logger/LogStructure.h
@@ -714,6 +714,21 @@ struct PACKED log_MotBatt {
     uint8_t mot_fail_flags;
 };
 
+struct PACKED log_VER {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint8_t board_type;
+    uint16_t board_subtype;
+    uint8_t major;
+    uint8_t minor;
+    uint8_t patch;
+    uint8_t fw_type;
+    uint32_t git_hash;
+    char fw_string[64];
+    uint16_t _APJ_BOARD_ID;
+};
+
+
 // FMT messages define all message formats other than FMT
 // UNIT messages define units which can be referenced by FMTU messages
 // FMTU messages associate types (e.g. centimeters/second/second) to FMT message fields
@@ -1374,6 +1389,8 @@ LOG_STRUCTURE_FROM_VISUALODOM \
 LOG_STRUCTURE_FROM_AIS, \
     { LOG_SCRIPTING_MSG, sizeof(log_Scripting), \
       "SCR",   "QNIii", "TimeUS,Name,Runtime,Total_mem,Run_mem", "s-sbb", "F-F--", true }, \
+    { 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 }
 
@@ -1457,6 +1474,7 @@ enum LogMessages : uint8_t {
     LOG_SCRIPTING_MSG,
     LOG_VIDEO_STABILISATION_MSG,
     LOG_MOTBATT_MSG,
+    LOG_VER_MSG,
 
     _LOG_LAST_MSG_
 };
diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp
index 9562743b22..6fdb504f92 100644
--- a/libraries/AP_Logger/LoggerMessageWriter.cpp
+++ b/libraries/AP_Logger/LoggerMessageWriter.cpp
@@ -242,9 +242,16 @@ void LoggerMessageWriter_WriteSysInfo::process() {
                 return; // call me again
             }
         }
-        stage = Stage::SYSTEM_ID;
+        stage = Stage::VER;
         FALLTHROUGH;
 
+    case Stage::VER: {
+        if (!_logger_backend->Write_VER()) {
+            return;
+        }
+        stage = Stage::SYSTEM_ID;
+        FALLTHROUGH;
+    }
     case Stage::SYSTEM_ID:
         char sysid[40];
         if (hal.util->get_system_id(sysid)) {
diff --git a/libraries/AP_Logger/LoggerMessageWriter.h b/libraries/AP_Logger/LoggerMessageWriter.h
index b9a4d5b210..977643f647 100644
--- a/libraries/AP_Logger/LoggerMessageWriter.h
+++ b/libraries/AP_Logger/LoggerMessageWriter.h
@@ -31,6 +31,7 @@ private:
     enum class Stage : uint8_t {
         FIRMWARE_STRING = 0,
         GIT_VERSIONS,
+        VER,  // i.e. the "VER" message
         SYSTEM_ID,
         PARAM_SPACE_USED,
         RC_PROTOCOL