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

AP_Vehicle: implement INS_HNTCH_FM_RAT

Browse Source 
this allows for a throttle based harmonic notch min frequency ratio,
allowing for the notch to go below the configured frequency at low
throttle

This is important for quadplanes, but will also benefit multirotors
flying at lower throttle due to lower payload or when descending

This also disables the throttle based harmonic notch when motors are
in SHUT_DOWN state
This commit is contained in:
Andrew Tridgell 2022-06-11 06:42:38 +10:00
parent 366aa7154c
commit 50740124fe
2 changed files with 29 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -396,6 +396,27 @@ bool AP_Vehicle::is_crashed() const
return AP::arming().last_disarm_method() == AP_Arming::Method::CRASH;
}
// update the harmonic notch filter for throttle based notch
void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch)
{
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI
const float ref_freq = notch.params.center_freq_hz();
const float ref = notch.params.reference();
const float min_ratio = notch.params.freq_min_ratio();
const AP_Motors* motors = AP::motors();
if (motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) {
notch.set_inactive(true);
} else {
notch.set_inactive(false);
}
const float motors_throttle = motors != nullptr ? MAX(0,motors->get_throttle_out()) : 0;
float throttle_freq = ref_freq * MAX(min_ratio, sqrtf(motors_throttle / ref));
notch.update_freq_hz(throttle_freq);
#endif
}
// update the harmonic notch filter center frequency dynamically
void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
{
@ -410,14 +431,10 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
return;
}
const AP_Motors* motors = AP::motors();
const float motors_throttle = motors != nullptr ? MAX(0,motors->get_throttle_out()) : 0;
const float throttle_freq = ref_freq * MAX(1.0f, sqrtf(motors_throttle / ref));
switch (notch.params.tracking_mode()) {
case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
// set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
notch.update_freq_hz(throttle_freq);
update_throttle_notch(notch);
break;
case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
@ -446,7 +463,7 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
if (num_notches > 0) {
notch.update_frequencies_hz(num_notches, notches);
} else { // throttle fallback
notch.update_freq_hz(throttle_freq);
update_throttle_notch(notch);
}
} else {
notch.update_freq_hz(MAX(ref_freq, AP::esc_telem().get_average_motor_frequency_hz() * ref));

9
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -250,9 +250,6 @@ public:
#endif // AP_SCRIPTING_ENABLED
// update the harmonic notch
void update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch);
// zeroing the RC outputs can prevent unwanted motor movement:
virtual bool should_zero_rc_outputs_on_reboot() const { return false; }
@ -416,6 +413,12 @@ private:
// statustext:
void send_watchdog_reset_statustext();
// update the harmonic notch for throttle based notch
void update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch);
// update the harmonic notch
void update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch);
// run notch update at either loop rate or 200Hz
void update_dynamic_notch_at_specified_rate();