AP_Vehicle: link in AP_Filter support

allow filters to be compiled out
add filter updates at 1Hz
This commit is contained in:
Andy Piper 2023-10-20 13:31:40 +01:00 committed by Andrew Tridgell
parent e729c8ccfa
commit 6ecb18e027
2 changed files with 16 additions and 0 deletions

View File

@ -241,6 +241,11 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = {
#endif
#endif // AP_NETWORKING_ENABLED
#if AP_FILTER_ENABLED
// @Group: FILT
// @Path: ../Filter/AP_Filter.cpp
AP_SUBGROUPINFO(filters, "FILT", 26, AP_Vehicle, AP_Filters),
#endif
AP_GROUPEND
};
@ -419,6 +424,10 @@ void AP_Vehicle::setup()
custom_rotations.init();
#if AP_FILTER_ENABLED
filters.init();
#endif
#if HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED
for (uint8_t i = 0; i<ESC_TELEM_MAX_ESCS; i++) {
esc_noise[i].set_cutoff_frequency(2);
@ -558,6 +567,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
#endif
#if HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED
SCHED_TASK(check_motor_noise, 5, 50, 252),
#endif
#if AP_FILTER_ENABLED && !APM_BUILD_TYPE(APM_BUILD_AP_Periph)
SCHED_TASK_CLASS(AP_Filters, &vehicle.filters, update, 1, 100, 252),
#endif
SCHED_TASK(update_arming, 1, 50, 253),
};

View File

@ -62,6 +62,7 @@
#include <AP_CheckFirmware/AP_CheckFirmware.h>
#include <Filter/LowPassFilter.h>
#include <AP_KDECAN/AP_KDECAN.h>
#include <Filter/AP_Filter.h>
class AP_DDS_Client;
@ -469,6 +470,9 @@ private:
uint32_t _last_internal_errors; // backup of AP_InternalError::internal_errors bitmask
AP_CustomRotations custom_rotations;
#if AP_FILTER_ENABLED
AP_Filters filters;
#endif
// Bitmask of modes to disable from gcs
AP_Int32 flight_mode_GCS_block;