From d2a683a75881b2c11696be51a3e9813eef2cbae1 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 25 Mar 2019 12:04:59 +1100
Subject: [PATCH] AP_Logger: move Log_Write_Error into library

---
 libraries/AP_Logger/AP_Logger.cpp  | 13 ++++++
 libraries/AP_Logger/AP_Logger.h    | 69 ++++++++++++++++++++++++++++++
 libraries/AP_Logger/LogStructure.h | 15 ++++++-
 3 files changed, 96 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp
index 6bbeba506e..091b87a08a 100644
--- a/libraries/AP_Logger/AP_Logger.cpp
+++ b/libraries/AP_Logger/AP_Logger.cpp
@@ -1096,6 +1096,19 @@ void AP_Logger::Write_Event(Log_Event id)
     WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
+// Write an error packet
+void AP_Logger::Write_Error(LogErrorSubsystem sub_system,
+                            LogErrorCode error_code)
+{
+  struct log_Error pkt = {
+      LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG),
+      time_us       : AP_HAL::micros64(),
+      sub_system    : uint8_t(sub_system),
+      error_code    : uint8_t(error_code),
+  };
+  WriteCriticalBlock(&pkt, sizeof(pkt));
+}
+
 namespace AP {
 
 AP_Logger &logger()
diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h
index 1078c32f0a..9e7394fd72 100644
--- a/libraries/AP_Logger/AP_Logger.h
+++ b/libraries/AP_Logger/AP_Logger.h
@@ -98,6 +98,73 @@ enum Log_Event : uint8_t {
     DATA_NOT_BOTTOMED = 166,
 };
 
+enum class LogErrorSubsystem : uint8_t {
+    MAIN = 1,
+    RADIO = 2,
+    COMPASS = 3,
+    OPTFLOW = 4,   // not used
+    FAILSAFE_RADIO = 5,
+    FAILSAFE_BATT = 6,
+    FAILSAFE_GPS = 7,   // not used
+    FAILSAFE_GCS = 8,
+    FAILSAFE_FENCE = 9,
+    FLIGHT_MODE = 10,
+    GPS = 11,
+    CRASH_CHECK = 12,
+    FLIP = 13,
+    AUTOTUNE = 14,  // not used
+    PARACHUTES = 15,
+    EKFCHECK = 16,
+    FAILSAFE_EKFINAV = 17,
+    BARO = 18,
+    CPU = 19,
+    FAILSAFE_ADSB = 20,
+    TERRAIN = 21,
+    NAVIGATION = 22,
+    FAILSAFE_TERRAIN = 23,
+    EKF_PRIMARY = 24,
+    THRUST_LOSS_CHECK = 25,
+};
+
+// bizarrely this enumeration has lots of duplicate values, offering
+// very little in the way of typesafety
+enum class LogErrorCode : uint8_t {
+// general error codes
+    ERROR_RESOLVED  = 0,
+    FAILED_TO_INITIALISE = 1,
+    UNHEALTHY = 4,
+// subsystem specific error codes -- radio
+    RADIO_LATE_FRAME = 2,
+// subsystem specific error codes -- failsafe_thr, batt, gps
+    FAILSAFE_RESOLVED = 0,
+    FAILSAFE_OCCURRED = 1,
+// subsystem specific error codes -- main
+    MAIN_INS_DELAY = 1,
+// subsystem specific error codes -- crash checker
+    CRASH_CHECK_CRASH = 1,
+    CRASH_CHECK_LOSS_OF_CONTROL = 2,
+// subsystem specific error codes -- flip
+    FLIP_ABANDONED = 2,
+// subsystem specific error codes -- terrain
+    MISSING_TERRAIN_DATA = 2,
+// subsystem specific error codes -- navigation
+    FAILED_TO_SET_DESTINATION = 2,
+    RESTARTED_RTL = 3,
+    FAILED_CIRCLE_INIT = 4,
+    DEST_OUTSIDE_FENCE = 5,
+
+// parachute failed to deploy because of low altitude or landed
+    PARACHUTE_TOO_LOW = 2,
+    PARACHUTE_LANDED = 3,
+// EKF check definitions
+    EKFCHECK_BAD_VARIANCE = 2,
+    EKFCHECK_VARIANCE_CLEARED = 0,
+// Baro specific error codes
+    BARO_GLITCH = 2,
+// GPS specific error coces
+    GPS_GLITCH = 2,
+};
+
 // fwd declarations to avoid include errors
 class AC_AttitudeControl;
 class AC_PosControl;
@@ -150,6 +217,8 @@ public:
 
     void Write_Parameter(const char *name, float value);
     void Write_Event(Log_Event id);
+    void Write_Error(LogErrorSubsystem sub_system,
+                     LogErrorCode error_code);
     void Write_GPS(uint8_t instance, uint64_t time_us=0);
     void Write_RFND(const RangeFinder &rangefinder);
     void Write_IMU();
diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h
index c5790092d2..a108525b94 100644
--- a/libraries/AP_Logger/LogStructure.h
+++ b/libraries/AP_Logger/LogStructure.h
@@ -163,6 +163,14 @@ struct PACKED log_Event {
     uint64_t time_us;
     uint8_t id;
 };
+
+struct PACKED log_Error {
+  LOG_PACKET_HEADER;
+  uint64_t time_us;
+  uint8_t sub_system;
+  uint8_t error_code;
+};
+
 struct PACKED log_GPS {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -1490,7 +1498,10 @@ Format characters in the format string for binary log messages
     { LOG_EVENT_MSG, sizeof(log_Event), \
       "EV",   "QB",           "TimeUS,Id", "s-", "F-" }, \
     { LOG_MSG_SBPEVENT, sizeof(log_SbpEvent), \
-      "SBRE", "QHIiBB", "TimeUS,GWk,GMS,ns_residual,level,quality", "s?????", "F?????" }
+      "SBRE", "QHIiBB", "TimeUS,GWk,GMS,ns_residual,level,quality", "s?????", "F?????" }, \
+    { LOG_ERROR_MSG, sizeof(log_Error), \
+      "ERR",   "QBB",         "TimeUS,Subsys,ECode", "s--", "F--" }
+
 // #endif
 
 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES, LOG_SBP_STRUCTURES
@@ -1648,6 +1659,8 @@ enum LogMessages : uint8_t {
     LOG_EVENT_MSG,
     LOG_WHEELENCODER_MSG,
     LOG_MAV_MSG,
+    LOG_ERROR_MSG,
+
     _LOG_LAST_MSG_
 };