mirror of https://github.com/ArduPilot/ardupilot
Plane: moved harmonic notch update code to AP_Vehicle
This commit is contained in:
parent
fc1aa44c0a
commit
0faacbd0c6
|
@ -1501,7 +1501,7 @@ void Plane::load_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" },
|
||||
|
@ -1513,11 +1513,9 @@ void Plane::load_parameters(void)
|
|||
for (uint8_t i=0; i<notchfilt_table_size; i++) {
|
||||
AP_Param::convert_old_parameters(¬chfilt_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);
|
||||
}
|
||||
}
|
||||
#endif // HAL_INS_NUM_HARMONIC_NOTCH_FILTERS
|
||||
|
||||
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
|
||||
|
|
|
@ -1056,7 +1056,6 @@ private:
|
|||
void startup_INS_ground(void);
|
||||
bool should_log(uint32_t mask);
|
||||
int8_t throttle_percentage(void);
|
||||
void update_dynamic_notch(uint8_t idx) override;
|
||||
void notify_mode(const Mode& mode);
|
||||
|
||||
// takeoff.cpp
|
||||
|
|
|
@ -445,83 +445,3 @@ int8_t Plane::throttle_percentage(void)
|
|||
}
|
||||
return constrain_int16(throttle, -100, 100);
|
||||
}
|
||||
|
||||
// update the harmonic notch filter center frequency dynamically
|
||||
void Plane::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;
|
||||
}
|
||||
|
||||
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
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
if (quadplane.available()) {
|
||||
ins.update_harmonic_notch_freq_hz(idx, ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
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/60.0)));
|
||||
} else {
|
||||
ins.update_harmonic_notch_freq_hz(idx, ref_freq);
|
||||
}
|
||||
break;
|
||||
}
|
||||
#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);
|
||||
#if HAL_QUADPLANE_ENABLED
|
||||
} else if (quadplane.available()) { // throttle fallback
|
||||
ins.update_harmonic_notch_freq_hz(idx, ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
|
||||
#endif
|
||||
} else {
|
||||
ins.update_harmonic_notch_freq_hz(idx, ref_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;
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue