ArduCopter: allow HarmonicNotches to be compiled out of the code

This commit is contained in:
Peter Barker 2024-02-13 11:26:07 +11:00 committed by Andrew Tridgell
parent bafd73ba14
commit 4569b1ba14
2 changed files with 6 additions and 0 deletions

View File

@ -522,9 +522,11 @@ void Copter::loop_rate_logging()
Log_Write_Attitude();
Log_Write_PIDS(); // only logs if PIDS bitmask is set
}
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
if (should_log(MASK_LOG_FTN_FAST)) {
AP::ins().write_notch_log_messages();
}
#endif
if (should_log(MASK_LOG_IMU_FAST)) {
AP::ins().Write_IMU();
}
@ -764,7 +766,9 @@ void Copter::update_altitude()
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
if (!should_log(MASK_LOG_FTN_FAST)) {
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
AP::ins().write_notch_log_messages();
#endif
#if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages();
#endif

View File

@ -1493,6 +1493,7 @@ void Copter::convert_pid_parameters(void)
};
AP_Param::convert_old_parameters(&ff_and_filt_conversion_info[0], ARRAY_SIZE(ff_and_filt_conversion_info));
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1
if (!ins.harmonic_notches[1].params.enabled()) {
// notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch
@ -1508,6 +1509,7 @@ void Copter::convert_pid_parameters(void)
AP_Param::set_default_by_name("INS_HNTC2_HMNCS", 1);
}
#endif
#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
// ACRO_RP_P and ACRO_Y_P replaced with ACRO_RP_RATE and ACRO_Y_RATE for Copter-4.2
// PARAMETER_CONVERSION - Added: Sep-2021