diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index 2318766a6720a6..8401f73085f371 100644 --- 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 ¬ch : harmonic_notches) { const uint8_t i = ¬ch - &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]); }