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

AP_Vehicle: removed the clamping of notch filters at vehicle level 

moved the claiming down into HarmonicNotchFilter
This commit is contained in:
Andrew Tridgell 2023-11-02 20:59:41 +11:00
parent 72d235a8a8
commit e18983780f
1 changed files with 6 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -751,7 +751,6 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch)
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover) #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover)
const float ref_freq = notch.params.center_freq_hz(); const float ref_freq = notch.params.center_freq_hz();
const float ref = notch.params.reference(); const float ref = notch.params.reference();
const float min_ratio = notch.params.freq_min_ratio();
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
const AP_Motors* motors = AP::motors(); const AP_Motors* motors = AP::motors();
@ -761,7 +760,7 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch)
const float motors_throttle = motors != nullptr ? abs(motors->get_throttle() / 100.0f) : 0; const float motors_throttle = motors != nullptr ? abs(motors->get_throttle() / 100.0f) : 0;
#endif #endif
float throttle_freq = ref_freq * MAX(min_ratio, sqrtf(motors_throttle / ref)); float throttle_freq = ref_freq * sqrtf(MAX(0,motors_throttle) / ref);
notch.update_freq_hz(throttle_freq); notch.update_freq_hz(throttle_freq);
#endif #endif
@ -781,17 +780,6 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
return; return;
} }
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
const AP_Motors* motors = AP::motors();
if (motors != nullptr && motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) {
notch.set_inactive(true);
} else {
notch.set_inactive(false);
}
#else // APM_BUILD_Rover: keep notch active
notch.set_inactive(false);
#endif
switch (notch.params.tracking_mode()) { switch (notch.params.tracking_mode()) {
case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
// set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
@ -806,9 +794,9 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
float rpm; float rpm;
if (rpm_sensor != nullptr && rpm_sensor->get_rpm(sensor, rpm)) { if (rpm_sensor != nullptr && rpm_sensor->get_rpm(sensor, rpm)) {
// set the harmonic notch filter frequency from the main rotor rpm // set the harmonic notch filter frequency from the main rotor rpm
notch.update_freq_hz(MAX(ref_freq * notch.params.freq_min_ratio(), rpm * ref * (1.0/60))); notch.update_freq_hz(rpm * ref * (1.0/60));
} else { } else {
notch.update_freq_hz(ref_freq); notch.update_freq_hz(0);
} }
break; break;
} }
@ -820,18 +808,13 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
float notches[INS_MAX_NOTCHES]; float notches[INS_MAX_NOTCHES];
// ESC telemetry will return 0 for missing data, but only after 1s // ESC telemetry will return 0 for missing data, but only after 1s
const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches); const uint8_t num_notches = AP::esc_telem().get_motor_frequencies_hz(INS_MAX_NOTCHES, notches);
for (uint8_t i = 0; i < num_notches; i++) {
if (!is_zero(notches[i])) {
notches[i] = MAX(ref_freq, notches[i]);
}
}
if (num_notches > 0) { if (num_notches > 0) {
notch.update_frequencies_hz(num_notches, notches); notch.update_frequencies_hz(num_notches, notches);
} else { // throttle fallback } else { // throttle fallback
update_throttle_notch(notch); update_throttle_notch(notch);
} }
} else { } else {
notch.update_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref)); notch.update_freq_hz(AP::esc_telem().get_average_motor_frequency_hz() * ref);
} }
break; break;
#endif #endif
@ -843,20 +826,14 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches); const uint8_t peaks = gyro_fft.get_weighted_noise_center_frequencies_hz(notch.num_dynamic_notches, notches);
if (peaks > 0) { if (peaks > 0) {
for (uint8_t i = 0; i < peaks; i++) { notch.set_inactive(false);
notches[i] = MAX(ref_freq, notches[i]);
}
notch.update_frequencies_hz(peaks, notches); notch.update_frequencies_hz(peaks, notches);
} else { // since FFT can be used post-filter it is better to disable the notch when there is no data } else { // since FFT can be used post-filter it is better to disable the notch when there is no data
notch.set_inactive(true); notch.set_inactive(true);
} }
} else { } else {
float center_freq = gyro_fft.get_weighted_noise_center_freq_hz(); float center_freq = gyro_fft.get_weighted_noise_center_freq_hz();
if (!is_zero(center_freq)) { notch.update_freq_hz(center_freq);
notch.update_freq_hz(MAX(ref_freq, center_freq));
} else { // since FFT can be used post-filter it is better to disable the notch when there is no data
notch.set_inactive(true);
}
} }
break; break;
#endif #endif