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

AP_Vehicle: enable HNTCH for Rover

This commit is contained in:
Yuri 2022-10-21 19:30:15 -05:00 committed by Randy Mackay
parent 33cbf74ef5
commit 9e4b7ddcfe
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -10,6 +10,7 @@
#include <AP_RPM/AP_RPM.h>
#include <SRV_Channel/SRV_Channel.h>
#include <AP_Motors/AP_Motors.h>
#include <AR_Motors/AP_MotorsUGV.h>
#include <AP_CheckFirmware/AP_CheckFirmware.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <AP_HAL_ChibiOS/sdcard.h>
@ -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 &notch)
{
#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 &notch)
// update the harmonic notch filter center frequency dynamically
void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
{
#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 &notch)
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 &notch)
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()
{