From ad9a8c8bd98d2b7fd81c4c2acdc9b766310aebae Mon Sep 17 00:00:00 2001
From: Siddharth Purohit <siddharthbharatpurohit@gmail.com>
Date: Thu, 20 May 2021 14:15:24 +0530
Subject: [PATCH] AP_HAL_ChibiOS: record error states in CANFDIface and
 CANIface

---
 libraries/AP_HAL_ChibiOS/CANFDIface.cpp |  7 +++++--
 libraries/AP_HAL_ChibiOS/CANFDIface.h   |  1 +
 libraries/AP_HAL_ChibiOS/CANIface.h     |  1 +
 libraries/AP_HAL_ChibiOS/CanIface.cpp   | 10 +++++++---
 4 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
index 885caf31eb..b87b366193 100644
--- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
+++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
@@ -886,6 +886,7 @@ void CANIface::pollErrorFlagsFromISR()
     const uint8_t cel = can_->ECR >> 16;
 
     if (cel != 0) {
+        stats.ecr = can_->ECR;
         for (int i = 0; i < NumTxMailboxes; i++) {
             if (!pending_tx_[i].abort_on_error || pending_tx_[i].aborted) {
                 continue;
@@ -1041,7 +1042,8 @@ void CANIface::get_stats(ExpandingString &str)
                "rx_overflow:    %lu\n"
                "rx_errors:      %lu\n"
                "num_busoff_err: %lu\n"
-               "num_events:     %lu\n",
+               "num_events:     %lu\n"
+               "ECR:            %lx\n",
                stats.tx_requests,
                stats.tx_rejected,
                stats.tx_success,
@@ -1051,7 +1053,8 @@ void CANIface::get_stats(ExpandingString &str)
                stats.rx_overflow,
                stats.rx_errors,
                stats.num_busoff_err,
-               stats.num_events);
+               stats.num_events,
+               stats.ecr);
 }
 #endif
 
diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h
index e9218a5c79..0edde8450d 100644
--- a/libraries/AP_HAL_ChibiOS/CANFDIface.h
+++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h
@@ -159,6 +159,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
         uint32_t rx_errors;
         uint32_t num_busoff_err;
         uint32_t num_events;
+        uint32_t ecr;
     } stats;
 
 public:
diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h
index 19393256c4..0be1e28daa 100644
--- a/libraries/AP_HAL_ChibiOS/CANIface.h
+++ b/libraries/AP_HAL_ChibiOS/CANIface.h
@@ -153,6 +153,7 @@ class ChibiOS::CANIface : public AP_HAL::CANIface
         uint32_t rx_errors;
         uint32_t num_busoff_err;
         uint32_t num_events;
+        uint32_t esr;
     } stats;
 #endif
 
diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp
index 9727d32148..6e36c61484 100644
--- a/libraries/AP_HAL_ChibiOS/CanIface.cpp
+++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp
@@ -590,6 +590,9 @@ void CANIface::pollErrorFlagsFromISR()
 {
     const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
     if (lec != 0) {
+#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD)
+        stats.esr = can_->ESR; // Record error status
+#endif
         can_->ESR = 0;
 
         // Serving abort requests
@@ -950,7 +953,8 @@ void CANIface::get_stats(ExpandingString &str)
                "rx_overflow:    %lu\n"
                "rx_errors:      %lu\n"
                "num_busoff_err: %lu\n"
-               "num_events:     %lu\n",
+               "num_events:     %lu\n"
+               "ESR:            %lx\n",
                stats.tx_requests,
                stats.tx_rejected,
                stats.tx_success,
@@ -960,8 +964,8 @@ void CANIface::get_stats(ExpandingString &str)
                stats.rx_overflow,
                stats.rx_errors,
                stats.num_busoff_err,
-               stats.num_events);
-    memset(&stats, 0, sizeof(stats));
+               stats.num_events,
+               stats.esr);
 }
 #endif