AP_InertialSensor: call logging function for notch centers and harmonics

tridge · Feb 19, 2024 · c2bfc2a · c2bfc2a
1 parent 25989f2
commit c2bfc2a
Showing 1 changed file with 8 additions and 2 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
@@ -166,6 +166,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
 
 void AP_InertialSensor::write_notch_log_messages() const
 {
+    const uint64_t now_us = AP_HAL::micros64();
     for (auto &notch : harmonic_notches) {
         const uint8_t i = &notch - &harmonic_notches[0];
         if (!notch.params.enabled()) {
@@ -176,17 +177,22 @@ void AP_InertialSensor::write_notch_log_messages() const
             // log per motor center frequencies
             AP::logger().WriteStreaming(
                 "FTN", "TimeUS,I,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s#-zzzzzzzzzzzz", "F--------------", "QBBffffffffffff",
-                AP_HAL::micros64(),
+                now_us,
                 i,
                 notch.num_calculated_notch_frequencies,
                 notches[0], notches[1], notches[2], notches[3],
                 notches[4], notches[5], notches[6], notches[7],
                 notches[8], notches[9], notches[10], notches[11]);
+
+            // ask the HarmonicNotchFilter object for primary gyro to
+            // log the actual notch centers
+            const uint8_t primary_gyro = AP::ahrs().get_primary_gyro_index();
+            notch.filter[primary_gyro].log_notch_centers(i, now_us);
         } else {
             // log single center frequency
             AP::logger().WriteStreaming(
                 "FTNS", "TimeUS,I,NF", "s#z", "F--", "QBf",
-                AP_HAL::micros64(),
+                now_us,
                 i,
                 notches[0]);
         }