Sub: support MASK_LOG_IMU_FAST (loop rate logging)

ArduPilot · Dec 7, 2024 · 717b1d3 · 717b1d3
1 parent 9d21b33
commit 717b1d3
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git 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
@@ -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();