Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: call logging function for notch centers and harmonics

This commit is contained in:
Andrew Tridgell 2024-02-20 08:57:58 +11:00
parent fd1a260d46
commit d7d854a60f
1 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
View File

@ -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);
}
}