aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Vehicle: enable HNTCH for Rover

Browse Source
This commit is contained in:
Yuri 2022-10-21 19:30:15 -05:00 committed by Randy Mackay
parent cec53233dd
commit 4a9ac7ce1c
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 <AP_RPM/AP_RPM.h>
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include <AP_Motors/AP_Motors.h> #include <AP_Motors/AP_Motors.h>
#include <AR_Motors/AP_MotorsUGV.h>
#include <AP_CheckFirmware/AP_CheckFirmware.h> #include <AP_CheckFirmware/AP_CheckFirmware.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <AP_HAL_ChibiOS/sdcard.h> #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 // update the harmonic notch filter for throttle based notch
void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &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_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(); 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 AP_Motors* motors = AP::motors();
const float motors_throttle = motors != nullptr ? MAX(0,motors->get_throttle_out()) : 0; 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)); float throttle_freq = ref_freq * MAX(min_ratio, sqrtf(motors_throttle / ref));
notch.update_freq_hz(throttle_freq); 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 // update the harmonic notch filter center frequency dynamically
void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch) 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()) { if (!notch.params.enabled()) {
return; return;
} }
@ -484,12 +491,16 @@ 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(); const AP_Motors* motors = AP::motors();
if (motors != nullptr && motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) { if (motors != nullptr && motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) {
notch.set_inactive(true); notch.set_inactive(true);
} else { } else {
notch.set_inactive(false); 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
@ -550,10 +561,9 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch)
notch.update_freq_hz(ref_freq); notch.update_freq_hz(ref_freq);
break; 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 // run notch update at either loop rate or 200Hz
void AP_Vehicle::update_dynamic_notch_at_specified_rate() void AP_Vehicle::update_dynamic_notch_at_specified_rate()
{ {