From 60846826346828c83794d456f5461a730bc43bd2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 31 Dec 2021 11:32:17 +1100 Subject: [PATCH] AP_InertialSensor: move INS notch filter logging into INS --- .../AP_InertialSensor/AP_InertialSensor.h | 3 +++ .../AP_InertialSensor_Logging.cpp | 26 +++++++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index ab0d2546a0..2cc8521e4d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -276,6 +276,9 @@ public: return _harmonic_notch_filter.hasOption(option); } + // write out harmonic notch log messages + void write_notch_log_messages() const; + // indicate which bit in LOG_BITMASK indicates raw logging enabled void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index 0c6bbb6e6d..aa433ec665 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -128,3 +128,29 @@ bool AP_InertialSensor::BatchSampler::Write_ISBD() const return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt)); } + +// @LoggerMessage: FTN +// @Description: Filter Tuning Messages +// @Field: TimeUS: microseconds since system startup +// @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 +void AP_InertialSensor::write_notch_log_messages() const +{ + const float* notches = get_gyro_dynamic_notch_center_frequencies_hz(); + AP::logger().Write( + "FTN", "TimeUS,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s-zzzzzzzzzzzz", "F-------------", "QBffffffffffff", AP_HAL::micros64(), get_num_gyro_dynamic_notch_center_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]); +}