From dd3eec2a3886dcafafc908b4da57d6d787bbdc54 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 11 Jun 2019 07:55:30 +1000
Subject: [PATCH] AP_Logger: persist logging for 15s after disarm or arming
 failure

this will provide useful information in logs on disarms in flight or
reasons for arming failure
---
 libraries/AP_Logger/AP_Logger.cpp | 38 ++++++++++++++++++++++++++++++-
 libraries/AP_Logger/AP_Logger.h   | 14 +++++++-----
 2 files changed, 45 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp
index 55c4a18b53..b524a58721 100644
--- a/libraries/AP_Logger/AP_Logger.cpp
+++ b/libraries/AP_Logger/AP_Logger.cpp
@@ -22,6 +22,11 @@ extern const AP_HAL::HAL& hal;
 #define HAL_LOGGING_MAV_BUFSIZE  8
 #endif 
 
+// by default log for 15 seconds after disarming
+#ifndef HAL_LOGGER_ARM_PERSIST
+#define HAL_LOGGER_ARM_PERSIST 15
+#endif
+
 #ifndef HAL_LOGGING_BACKENDS_DEFAULT
 # ifdef HAL_LOGGING_DATAFLASH
 #  define HAL_LOGGING_BACKENDS_DEFAULT Backend_Type::BLOCK
@@ -509,10 +514,12 @@ void AP_Logger::backend_starting_new_log(const AP_Logger_Backend *backend)
 
 bool AP_Logger::should_log(const uint32_t mask) const
 {
+    bool armed = vehicle_is_armed();
+
     if (!(mask & _log_bitmask)) {
         return false;
     }
-    if (!vehicle_is_armed() && !log_while_disarmed()) {
+    if (!armed && !log_while_disarmed()) {
         return false;
     }
     if (in_log_download()) {
@@ -1141,6 +1148,35 @@ void AP_Logger::Write_Error(LogErrorSubsystem sub_system,
   WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
+/*
+  return true if we should log while disarmed
+ */
+bool AP_Logger::log_while_disarmed(void) const
+{
+    if (_force_log_disarmed) {
+        return true;
+    }
+    if (_params.log_disarmed != 0) {
+        return true;
+    }
+
+    uint32_t now = AP_HAL::millis();
+    uint32_t persist_ms = HAL_LOGGER_ARM_PERSIST*1000U;
+
+    // keep logging for HAL_LOGGER_ARM_PERSIST seconds after disarming
+    const uint32_t arm_change_ms = hal.util->get_last_armed_change();
+    if (!hal.util->get_soft_armed() && arm_change_ms != 0 && now - arm_change_ms < persist_ms) {
+        return true;
+    }
+
+    // keep logging for HAL_LOGGER_ARM_PERSIST seconds after an arming failure
+    if (_last_arming_failure_ms && now - _last_arming_failure_ms < persist_ms) {
+        return true;
+    }
+
+    return false;
+}
+
 namespace AP {
 
 AP_Logger &logger()
diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
index 2c0fa24653..661abb306f 100644
--- a/libraries/AP_Logger/AP_Logger.h
+++ b/libraries/AP_Logger/AP_Logger.h
@@ -312,12 +312,7 @@ public:
 
     // accesss to public parameters
     void set_force_log_disarmed(bool force_logging) { _force_log_disarmed = force_logging; }
-    bool log_while_disarmed(void) const {
-        if (_force_log_disarmed) {
-            return true;
-        }
-        return _params.log_disarmed != 0;
-    }
+    bool log_while_disarmed(void) const;
     uint8_t log_replay(void) const { return _params.log_replay; }
     
     vehicle_startup_message_Writer _vehicle_messages;
@@ -344,6 +339,10 @@ public:
     bool logging_enabled() const;
     bool logging_failed() const;
 
+    // notify logging subsystem of an arming failure. This triggers
+    // logging for HAL_LOGGER_ARM_PERSIST seconds
+    void arming_failure() { _last_arming_failure_ms = AP_HAL::millis(); }
+
     void set_vehicle_armed(bool armed_state);
     bool vehicle_is_armed() const { return _armed; }
 
@@ -505,6 +504,9 @@ private:
     GCS_MAVLINK *_log_sending_link;
     HAL_Semaphore_Recursive _log_send_sem;
 
+    // last time arming failed, for backends
+    uint32_t _last_arming_failure_ms;
+
     bool should_handle_log_message();
     void handle_log_message(class GCS_MAVLINK &, mavlink_message_t *msg);