From 6ecb18e02765a4a9ea86b86aeaadeef2e47f7cc2 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Fri, 20 Oct 2023 13:31:40 +0100
Subject: [PATCH] AP_Vehicle: link in AP_Filter support

allow filters to be compiled out
add filter updates at 1Hz
---
 libraries/AP_Vehicle/AP_Vehicle.cpp | 12 ++++++++++++
 libraries/AP_Vehicle/AP_Vehicle.h   |  4 ++++
 2 files changed, 16 insertions(+)

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index 9a4f702a2d..b04cc282fc 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -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),
 };
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index 18b43fc020..1b12f3505c 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -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;