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

AP_Vehicle: support per-motor throttle based notches

This commit is contained in:
Andy Piper 2024-04-02 21:03:54 +01:00 committed by Andrew Tridgell
parent e23e58fc17
commit fd19c3f401
1 changed files with 27 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -776,21 +776,43 @@ bool AP_Vehicle::is_crashed() const
// 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||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 = notch.params.reference();
#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 float motors_throttle = motors != nullptr ? MAX(0,motors->get_throttle_out()) : 0;
if (motors == nullptr) {
notch.update_freq_hz(0);
return;
}
const float motors_throttle = MAX(0,motors->get_throttle_out());
// set the harmonic notch filter frequency scaled on measured frequency
if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) {
float notches[INS_MAX_NOTCHES];
uint8_t motor_num = 0;
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
float motor_throttle = 0;
if (motors->get_thrust(i, motor_throttle)) {
notches[motor_num] = ref_freq * sqrtf(MAX(0, motor_throttle) / ref);
motor_num++;
}
if (motor_num >= INS_MAX_NOTCHES) {
break;
}
}
notch.update_frequencies_hz(motor_num, notches);
} else
#else // APM_BUILD_Rover
const AP_MotorsUGV *motors = AP::motors_ugv();
const float motors_throttle = motors != nullptr ? abs(motors->get_throttle() / 100.0f) : 0;
#endif
{
float throttle_freq = ref_freq * sqrtf(MAX(0,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
}