Copter: only log attitude and IMU from main loop

ArduPilot · Jun 6, 2024 · bd0fbb4 · bd0fbb4
1 parent b934967
commit bd0fbb4
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 12 deletions.
diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp
@@ -152,7 +152,7 @@ void Copter::rate_controller_thread()
         if (log_rate_decimate > 0 && log_loop_count++ >= log_rate_decimate) {
             log_loop_count = 0;
 #if HAL_LOGGING_ENABLED
-            fast_logging();
+            rate_controller_log_update();
 #endif
         }
 
@@ -243,6 +243,24 @@ void Copter::rate_controller_filter_update()
     ins.update_backend_filters();
 }
 
+/*
+  log only those items that are updated at the rate loop rate
+ */
+void Copter::rate_controller_log_update()
+{
+#if HAL_LOGGING_ENABLED
+    if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
+        Log_Write_Rate();
+        Log_Write_PIDS(); // only logs if PIDS bitmask is set
+    }
+#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
+    if (should_log(MASK_LOG_FTN_FAST)) {
+        AP::ins().write_notch_log_messages();
+    }
+#endif
+#endif
+}
+
 #endif
 
 /*

diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp
@@ -522,23 +522,22 @@ void Copter::update_batt_compass(void)
 // should be run at loop rate
 void Copter::loop_rate_logging()
 {
+    bool log_rates = true;
 #if AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED
     if (using_rate_thread && copter.g2.att_log_rate_hz != 0) {
-        return;
+        log_rates = false;
     }
 #endif
 
-    fast_logging();
-}
-
-void Copter::fast_logging()
-{
-    if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
+   if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
         Log_Write_Attitude();
-        Log_Write_PIDS(); // only logs if PIDS bitmask is set
+        if (log_rates) {
+            Log_Write_Rate();
+            Log_Write_PIDS(); // only logs if PIDS bitmask is set
+        }
     }
 #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
-    if (should_log(MASK_LOG_FTN_FAST)) {
+    if (should_log(MASK_LOG_FTN_FAST) && log_rates) {
         AP::ins().write_notch_log_messages();
     }
 #endif

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
@@ -700,7 +700,6 @@ class Copter : public AP_Vehicle {
     void throttle_loop();
     void update_batt_compass(void);
     void loop_rate_logging();
-    void fast_logging();
     void ten_hz_logging_loop();
     void twentyfive_hz_logging();
     void three_hz_loop();
@@ -724,6 +723,7 @@ class Copter : public AP_Vehicle {
     uint16_t get_pilot_speed_dn() const;
     void rate_controller_thread();
     void rate_controller_filter_update();
+    void rate_controller_log_update();
     void run_rate_controller_main();
 
 #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
@@ -851,6 +851,7 @@ class Copter : public AP_Vehicle {
     // Log.cpp
     void Log_Write_Control_Tuning();
     void Log_Write_Attitude();
+    void Log_Write_Rate();
     void Log_Write_EKF_POS();
     void Log_Write_PIDS();
     void Log_Write_Data(LogDataID id, int32_t value);

diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp
@@ -70,8 +70,12 @@ void Copter::Log_Write_Attitude()
     Vector3f targets = attitude_control->get_att_target_euler_cd();
     targets.z = wrap_360_cd(targets.z);
     ahrs.Write_Attitude(targets);
+}
+
+void Copter::Log_Write_Rate()
+{
     ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control);
- }
+}
 
 // Write PIDS packets
 void Copter::Log_Write_PIDS()