From d7d854a60fd1d8ad461cbdbbb06417eec47a2869 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 20 Feb 2024 08:57:58 +1100
Subject: [PATCH] AP_InertialSensor: call logging function for notch centers
 and harmonics

---
 .../AP_InertialSensor_Logging.cpp             | 38 +++++++++++--------
 1 file changed, 22 insertions(+), 16 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
index 2318766a67..8c20164f3e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
@@ -144,28 +144,29 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const
 // @Description: Filter Tuning Message - per motor
 // @Field: TimeUS: microseconds since system startup
 // @Field: I: instance
-// @Field: NDn: number of active dynamic harmonic notches
-// @Field: NF1: dynamic harmonic notch centre frequency for motor 1
-// @Field: NF2: dynamic harmonic notch centre frequency for motor 2
-// @Field: NF3: dynamic harmonic notch centre frequency for motor 3
-// @Field: NF4: dynamic harmonic notch centre frequency for motor 4
-// @Field: NF5: dynamic harmonic notch centre frequency for motor 5
-// @Field: NF6: dynamic harmonic notch centre frequency for motor 6
-// @Field: NF7: dynamic harmonic notch centre frequency for motor 7
-// @Field: NF8: dynamic harmonic notch centre frequency for motor 8
-// @Field: NF9: dynamic harmonic notch centre frequency for motor 9
-// @Field: NF10: dynamic harmonic notch centre frequency for motor 10
-// @Field: NF11: dynamic harmonic notch centre frequency for motor 11
-// @Field: NF12: dynamic harmonic notch centre frequency for motor 12
+// @Field: NDn: number of active harmonic notches
+// @Field: NF1: desired notch centre frequency for motor 1
+// @Field: NF2: desired notch centre frequency for motor 2
+// @Field: NF3: desired notch centre frequency for motor 3
+// @Field: NF4: desired notch centre frequency for motor 4
+// @Field: NF5: desired notch centre frequency for motor 5
+// @Field: NF6: desired notch centre frequency for motor 6
+// @Field: NF7: desired notch centre frequency for motor 7
+// @Field: NF8: desired notch centre frequency for motor 8
+// @Field: NF9: desired notch centre frequency for motor 9
+// @Field: NF10: desired notch centre frequency for motor 10
+// @Field: NF11: desired notch centre frequency for motor 11
+// @Field: NF12: desired notch centre frequency for motor 12
 
 // @LoggerMessage: FTNS
 // @Description: Filter Tuning Message
 // @Field: TimeUS: microseconds since system startup
 // @Field: I: instance
-// @Field: NF: dynamic harmonic notch centre frequency
+// @Field: NF: desired notch centre frequency
 
 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,7 +177,7 @@ 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],
@@ -186,10 +187,15 @@ void AP_InertialSensor::write_notch_log_messages() const
             // log single center frequency
             AP::logger().WriteStreaming(
                 "FTNS", "TimeUS,I,NF", "s#z", "F--", "QBf",
-                AP_HAL::micros64(),
+                now_us,
                 i,
                 notches[0]);
         }
+
+        // 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);
     }
 }