Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: moved harmonic notch update code to AP_Vehicle

Browse Source
This commit is contained in:
Andrew Tridgell 2022-04-15 17:40:01 +10:00
parent f8c1c39868
commit 2a5d662d84
3 changed files with 3 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -879,7 +879,6 @@ private:
// system.cpp
void init_ardupilot() override;
void startup_INS_ground();
void update_dynamic_notch(uint8_t idx) override;
bool position_ok() const;
bool ekf_has_absolute_position() const;
bool ekf_has_relative_position() const;

8
ArduCopter/Parameters.cpp
View File

@ -1388,7 +1388,7 @@ void Copter::convert_pid_parameters(void)
}
#if HAL_INS_NUM_HARMONIC_NOTCH_FILTERS > 1
if (!ins.gyro_harmonic_notch_enabled(1)) {
if (!ins.harmonic_notches[1].params.enabled()) {
// notch filter parameter conversions (moved to INS_HNTC2) for 4.2.x, converted from fixed notch
const AP_Param::ConversionInfo notchfilt_conversion_info[] {
{ Parameters::k_param_ins, 101, AP_PARAM_INT8, "INS_HNTC2_ENABLE" },
@ -1400,10 +1400,8 @@ void Copter::convert_pid_parameters(void)
for (uint8_t i=0; i<notchfilt_table_size; i++) {
AP_Param::convert_old_parameters(&notchfilt_conversion_info[i], 1.0f);
}
if (ins.gyro_harmonic_notch_enabled(1)) {
AP_Param::set_default_by_name("INS_HNTC2_MODE", 0);
AP_Param::set_default_by_name("INS_HNTC2_HMNCS", 1);
}
AP_Param::set_default_by_name("INS_HNTC2_MODE", 0);
AP_Param::set_default_by_name("INS_HNTC2_HMNCS", 1);
}
#endif

75
ArduCopter/system.cpp
View File

@ -233,81 +233,6 @@ void Copter::startup_INS_ground()
ahrs.reset();
}
// update the harmonic notch filter center frequency dynamically
void Copter::update_dynamic_notch(uint8_t idx)
{
if (!ins.gyro_harmonic_notch_enabled(idx)) {
return;
}
const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz(idx);
const float ref = ins.get_gyro_harmonic_notch_reference(idx);
if (is_zero(ref)) {
ins.update_harmonic_notch_freq_hz(idx, ref_freq);
return;
}
const float throttle_freq = ref_freq * MAX(1.0f, sqrtf(motors->get_throttle_out() / ref));
switch (ins.get_gyro_harmonic_notch_tracking_mode(idx)) {
case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
// set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
ins.update_harmonic_notch_freq_hz(idx, throttle_freq);
break;
#if RPM_ENABLED == ENABLED
case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
case HarmonicNotchDynamicMode::UpdateRPM2: {
uint8_t sensor = (ins.get_gyro_harmonic_notch_tracking_mode(idx)==HarmonicNotchDynamicMode::UpdateRPM?0:1);
float rpm;
if (rpm_sensor.get_rpm(sensor, rpm)) {
// set the harmonic notch filter frequency from the main rotor rpm
ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1.0/60)));
} else {
ins.update_harmonic_notch_freq_hz(idx, ref_freq);
}
break;
}
#endif
#if HAL_WITH_ESC_TELEM
case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
// set the harmonic notch filter frequency scaled on measured frequency
if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
float notches[INS_MAX_NOTCHES];
const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches);
for (uint8_t i = 0; i < num_notches; i++) {
notches[i] = MAX(ref_freq, notches[i]);
}
if (num_notches > 0) {
ins.update_harmonic_notch_frequencies_hz(idx, num_notches, notches);
} else { // throttle fallback
ins.update_harmonic_notch_freq_hz(idx, throttle_freq);
}
} else {
ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref));
}
break;
#endif
#if HAL_GYROFFT_ENABLED
case HarmonicNotchDynamicMode::UpdateGyroFFT: // FFT based tracking
// set the harmonic notch filter frequency scaled on measured frequency
if (ins.has_harmonic_option(idx, HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
float notches[INS_MAX_NOTCHES];
const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(ins.get_num_gyro_dynamic_notches(idx), notches);
ins.update_harmonic_notch_frequencies_hz(idx, peaks, notches);
} else {
ins.update_harmonic_notch_freq_hz(idx, gyro_fft.get_weighted_noise_center_freq_hz());
}
break;
#endif
case HarmonicNotchDynamicMode::Fixed: // static
default:
ins.update_harmonic_notch_freq_hz(idx, ref_freq);
break;
}
}
// position_ok - returns true if the horizontal absolute position is ok and home position is set
bool Copter::position_ok() const
{