From e07033903812d88e753406e2fa8405342b762857 Mon Sep 17 00:00:00 2001
From: MattKear <matthewnkear@gmail.com>
Date: Fri, 14 Jun 2024 17:56:14 +0100
Subject: [PATCH] AP_RPM: Improve rpm logging

---
 libraries/AP_RPM/AP_RPM.cpp     | 31 +++++++++++++++++--------------
 libraries/AP_RPM/LogStructure.h | 14 +++++++++-----
 2 files changed, 26 insertions(+), 19 deletions(-)

diff --git a/libraries/AP_RPM/AP_RPM.cpp b/libraries/AP_RPM/AP_RPM.cpp
index 79a2646030914..087dcabe291d4 100644
--- a/libraries/AP_RPM/AP_RPM.cpp
+++ b/libraries/AP_RPM/AP_RPM.cpp
@@ -214,9 +214,7 @@ void AP_RPM::update(void)
     }
 
 #if HAL_LOGGING_ENABLED
-    if (enabled(0) || enabled(1)) {
-        Log_RPM();
-    }
+    Log_RPM();
 #endif
 }
 
@@ -295,18 +293,23 @@ bool AP_RPM::arming_checks(size_t buflen, char *buffer) const
 #if HAL_LOGGING_ENABLED
 void AP_RPM::Log_RPM() const
 {
-    float rpm1 = -1, rpm2 = -1;
-
-    get_rpm(0, rpm1);
-    get_rpm(1, rpm2);
+    // update logging for each instance
+    for (uint8_t i=0; i<num_instances; i++) {
+        if (drivers[i] == nullptr || !enabled(i)) {
+            // don't log unused instances
+            continue;
+        }
 
-    const struct log_RPM pkt{
-        LOG_PACKET_HEADER_INIT(LOG_RPM_MSG),
-        time_us     : AP_HAL::micros64(),
-        rpm1        : rpm1,
-        rpm2        : rpm2
-    };
-    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+        const struct log_RPM pkt{
+            LOG_PACKET_HEADER_INIT(LOG_RPM_MSG),
+            time_us     : AP_HAL::micros64(),
+            inst        : i,
+            rpm         : state[i].rate_rpm,
+            quality     : get_signal_quality(i),
+            health      : uint8_t(healthy(i))
+        };
+        AP::logger().WriteBlock(&pkt, sizeof(pkt));
+    }
 }
 #endif
 
diff --git a/libraries/AP_RPM/LogStructure.h b/libraries/AP_RPM/LogStructure.h
index d870fabeda758..e0677f343adbd 100644
--- a/libraries/AP_RPM/LogStructure.h
+++ b/libraries/AP_RPM/LogStructure.h
@@ -8,15 +8,19 @@
 // @LoggerMessage: RPM
 // @Description: Data from RPM sensors
 // @Field: TimeUS: Time since system startup
-// @Field: rpm1: First sensor's data
-// @Field: rpm2: Second sensor's data
+// @Field: I: Instance
+// @Field: RPM: Sensor's rpm measurement
+// @Field: Qual: Signal quality
+// @Field: H: Sensor Health (Bool)
 struct PACKED log_RPM {
     LOG_PACKET_HEADER;
     uint64_t time_us;
-    float rpm1;
-    float rpm2;
+    uint8_t inst;
+    float rpm;
+    float quality;
+    uint8_t health;
 };
 
 #define LOG_STRUCTURE_FROM_RPM        \
     { LOG_RPM_MSG, sizeof(log_RPM), \
-      "RPM",  "Qff", "TimeUS,rpm1,rpm2", "sqq", "F00" , true },
+      "RPM",  "QBffB", "TimeUS,I,RPM,Qual,H", "s#q--", "F-000" , true },