diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 5282e01e2f..807f825597 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include @@ -457,13 +458,19 @@ bool AP_Vehicle::is_crashed() const // update the harmonic notch filter for throttle based notch void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch) { -#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI +#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(); const float min_ratio = notch.params.freq_min_ratio(); +#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; +#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 * MAX(min_ratio, sqrtf(motors_throttle / ref)); notch.update_freq_hz(throttle_freq); @@ -473,7 +480,7 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch) // update the harmonic notch filter center frequency dynamically void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) { -#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover) if (!notch.params.enabled()) { return; } @@ -484,12 +491,16 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) 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()) { case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking @@ -550,10 +561,9 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) notch.update_freq_hz(ref_freq); break; } -#endif // APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI +#endif // APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI||APM_BUILD_TYPE(APM_BUILD_Rover) } - // run notch update at either loop rate or 200Hz void AP_Vehicle::update_dynamic_notch_at_specified_rate() {