From 4300ee3d1c0a02959acd53fbf559802482225df4 Mon Sep 17 00:00:00 2001
From: Clyde McQueen <clyde@mcqueen.net>
Date: Mon, 25 Nov 2024 15:12:57 -0800
Subject: [PATCH] Sub: support MASK_LOG_IMU_FAST (loop rate logging)

---
 ArduSub/ArduSub.cpp | 11 ++++++++++-
 ArduSub/Sub.h       |  1 +
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index 002fbfbaad591..e2923db562ff3 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -91,6 +91,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
 #if HAL_LOGGING_ENABLED
     SCHED_TASK(ten_hz_logging_loop,   10,    350,  51),
     SCHED_TASK(twentyfive_hz_logging, 25,    110,  54),
+    SCHED_TASK(loop_rate_logging, LOOP_RATE, 50,   55),
     SCHED_TASK_CLASS(AP_Logger,           &sub.logger,       periodic_tasks,     400, 300,  57),
 #endif
     SCHED_TASK_CLASS(AP_InertialSensor,   &sub.ins,          periodic,           400,  50,  60),
@@ -234,7 +235,15 @@ void Sub::twentyfive_hz_logging()
     }
 
     // log IMU data if we're not already logging at the higher rate
-    if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_RAW)) {
+    if (should_log(MASK_LOG_IMU) && !should_log(MASK_LOG_IMU_FAST)) {
+        AP::ins().Write_IMU();
+    }
+}
+
+// Full rate logging of IMU
+void Sub::loop_rate_logging()
+{
+    if (should_log(MASK_LOG_IMU_FAST)) {
         AP::ins().Write_IMU();
     }
 }
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 9c6e621bc45f1..0b63007dd305b 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -394,6 +394,7 @@ class Sub : public AP_Vehicle {
     void update_batt_compass(void);
     void ten_hz_logging_loop();
     void twentyfive_hz_logging();
+    void loop_rate_logging();
     void three_hz_loop();
     void one_hz_loop();
     void update_turn_counter();