From e729c8ccfa28bcb2a610e8bddd5e25a231c853fd Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 20 Oct 2023 13:31:27 +0100 Subject: [PATCH] Filter: add AP_Filter support enable filters with AP_FILTER_ENABLED allow filters to be compiled out add dynamic updates, remove load/save and enable --- libraries/Filter/AP_Filter.cpp | 155 +++++++++++++++++++++ libraries/Filter/AP_Filter.h | 92 ++++++++++++ libraries/Filter/AP_Filter_config.h | 15 ++ libraries/Filter/AP_Filter_params.cpp | 25 ++++ libraries/Filter/AP_NotchFilter_params.cpp | 53 +++++++ 5 files changed, 340 insertions(+) create mode 100644 libraries/Filter/AP_Filter.cpp create mode 100644 libraries/Filter/AP_Filter.h create mode 100644 libraries/Filter/AP_Filter_config.h create mode 100644 libraries/Filter/AP_Filter_params.cpp create mode 100644 libraries/Filter/AP_NotchFilter_params.cpp diff --git a/libraries/Filter/AP_Filter.cpp b/libraries/Filter/AP_Filter.cpp new file mode 100644 index 0000000000..ce65bd61ff --- /dev/null +++ b/libraries/Filter/AP_Filter.cpp @@ -0,0 +1,155 @@ +#include "AP_Filter.h" +#include +#include +#include + +#if AP_FILTER_ENABLED && !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_Filters::var_info[] = { + +#if AP_FILTER_NUM_FILTERS >= 1 + // @Group: 1_ + // @Path: AP_Filter_params.cpp + AP_SUBGROUPINFO(params[0], "1_", 2, AP_Filters, AP_Filter_params), + // @Group: 1_ + // @Path: AP_NotchFilter_params.cpp + AP_SUBGROUPVARPTR(filters[0], "1_", 3, AP_Filters, backend_var_info[0]), +#endif +#if AP_FILTER_NUM_FILTERS >= 2 + // @Group: 2_ + // @Path: AP_Filter_params.cpp + AP_SUBGROUPINFO(params[1], "2_", 4, AP_Filters, AP_Filter_params), + // @Group: 2_ + // @Path: AP_NotchFilter_params.cpp + AP_SUBGROUPVARPTR(filters[1], "2_", 5, AP_Filters, backend_var_info[1]), +#endif +#if AP_FILTER_NUM_FILTERS >= 3 + // @Group: 3_ + // @Path: AP_Filter_params.cpp + AP_SUBGROUPINFO(params[2], "3_", 6, AP_Filters, AP_Filter_params), + // @Group: 3_ + // @Path: AP_NotchFilter_params.cpp + AP_SUBGROUPVARPTR(filters[2], "3_", 7, AP_Filters, backend_var_info[2]), +#endif +#if AP_FILTER_NUM_FILTERS >= 4 + // @Group: 4_ + // @Path: AP_Filter_params.cpp + AP_SUBGROUPINFO(params[3], "4_", 8, AP_Filters, AP_Filter_params), + // @Group: 4_ + // @Path: AP_NotchFilter_params.cpp + AP_SUBGROUPVARPTR(filters[3], "4_", 9, AP_Filters, backend_var_info[3]), +#endif +#if AP_FILTER_NUM_FILTERS >= 5 + // @Group: 5_ + // @Path: AP_Filter_params.cpp + AP_SUBGROUPINFO(params[4], "5_", 10, AP_Filters, AP_Filter_params), + // @Group: 5_ + // @Path: AP_NotchFilter_params.cpp + AP_SUBGROUPVARPTR(filters[4], "5_", 11, AP_Filters, backend_var_info[4]), +#endif +#if AP_FILTER_NUM_FILTERS >= 6 + // @Group: 6_ + // @Path: AP_Filter_params.cpp + AP_SUBGROUPINFO(params[5], "6_", 12, AP_Filters, AP_Filter_params), + // @Group: 6_ + // @Path: AP_NotchFilter_params.cpp + AP_SUBGROUPVARPTR(filters[5], "6_", 13, AP_Filters, backend_var_info[5]), +#endif +#if AP_FILTER_NUM_FILTERS >= 7 + // @Group: 7_ + // @Path: AP_Filter_params.cpp + AP_SUBGROUPINFO(params[6], "7_", 14, AP_Filters, AP_Filter_params), + // @Group: 7_ + // @Path: AP_NotchFilter_params.cpp + AP_SUBGROUPVARPTR(filters[6], "7_", 15, AP_Filters, backend_var_info[6]), +#endif +#if AP_FILTER_NUM_FILTERS >= 8 + // @Group: 8_ + // @Path: AP_Filter_params.cpp + AP_SUBGROUPINFO(params[7], "8_", 16, AP_Filters, AP_Filter_params), + // @Group: 8_ + // @Path: AP_NotchFilter_params.cpp + AP_SUBGROUPVARPTR(filters[7], "8_", 17, AP_Filters, backend_var_info[7]), +#endif + AP_GROUPEND +}; + +const AP_Param::GroupInfo *AP_Filters::backend_var_info[AP_FILTER_NUM_FILTERS]; + +AP_Filter::AP_Filter(FilterType type): + _type(type) +{ +} + +AP_Filters::AP_Filters() +{ + AP_Param::setup_object_defaults(this, var_info); + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (singleton != nullptr) { + AP_HAL::panic("AP_Filters must be singleton"); + } +#endif + singleton = this; +} + +void AP_Filters::init() +{ + update(); +} + +// 1Hz update to process config changes +void AP_Filters::update() +{ + if (hal.util->get_soft_armed()) { + return; + } + + // make sure all filters are allocated + for (uint8_t i = 0; i < AP_FILTER_NUM_FILTERS; i++) { + bool update = false; + switch (AP_Filter::FilterType(params[i]._type)) { + case AP_Filter::FilterType::FILTER_NONE: + break; + case AP_Filter::FilterType::FILTER_NOTCH: + if (filters[i] == nullptr) { + filters[i] = new AP_NotchFilter_params(); + backend_var_info[i] = AP_NotchFilter_params::var_info; + update = true; + } + break; + default: + return; + } + + if (update) { + AP_Param::load_object_from_eeprom(filters[i], backend_var_info[i]); + AP_Param::invalidate_count(); + } + } +} + +AP_Filter* AP_Filters::get_filter(uint8_t index) +{ + if (index >= AP_FILTER_NUM_FILTERS) { + return nullptr; + } + + return filters[index-1]; +} + +// singleton instance +AP_Filters *AP_Filters::singleton; + +namespace AP { + +AP_Filters &filters() +{ + return *AP_Filters::get_singleton(); +} + +} + +#endif // AP_FILTER_ENABLED && !APM_BUILD_TYPE(APM_BUILD_AP_Periph) diff --git a/libraries/Filter/AP_Filter.h b/libraries/Filter/AP_Filter.h new file mode 100644 index 0000000000..7c303a0842 --- /dev/null +++ b/libraries/Filter/AP_Filter.h @@ -0,0 +1,92 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include +#include +#include "AP_Filter_config.h" +#include "NotchFilter.h" + +#if AP_FILTER_ENABLED + +class AP_Filter { +public: + enum class FilterType : uint8_t { + FILTER_NONE = 0, + FILTER_NOTCH = 1, + }; + + AP_Filter(FilterType type); + + void init(); + + virtual bool setup_notch_filter(NotchFilterFloat& filter, float sample_rate) { return false; } + + FilterType _type; +}; + +struct AP_Filter_params { +public: + AP_Filter_params(); + + static const struct AP_Param::GroupInfo var_info[]; + + AP_Enum _type; +}; + +struct AP_NotchFilter_params : public AP_Filter { +public: + AP_NotchFilter_params(); + + bool setup_notch_filter(NotchFilterFloat& filter, float sample_rate) override; + + static const struct AP_Param::GroupInfo var_info[]; + + AP_Float _center_freq_hz; + AP_Float _quality; + AP_Float _attenuation_dB; +}; + +class AP_Filters { +public: + AP_Filters(); + + CLASS_NO_COPY(AP_Filters); + + static AP_Filters *get_singleton(void) { return singleton; } + + void init(); + // 1Hz update to process config changes + void update(); + + static const struct AP_Param::GroupInfo var_info[]; + + static const struct AP_Param::GroupInfo *backend_var_info[AP_FILTER_NUM_FILTERS]; + + AP_Filter* get_filter(uint8_t filt_num); + +private: + AP_Filter* filters[AP_FILTER_NUM_FILTERS]; + AP_Filter_params params[AP_FILTER_NUM_FILTERS]; + + static AP_Filters *singleton; +}; + +namespace AP { + AP_Filters &filters(); +}; + +#endif // AP_FILTER_ENABLED + diff --git a/libraries/Filter/AP_Filter_config.h b/libraries/Filter/AP_Filter_config.h new file mode 100644 index 0000000000..d94dd9df1b --- /dev/null +++ b/libraries/Filter/AP_Filter_config.h @@ -0,0 +1,15 @@ +#pragma once + +#include + +#ifndef AP_FILTER_NUM_FILTERS +#if BOARD_FLASH_SIZE > 1024 +#define AP_FILTER_NUM_FILTERS 8 +#else +#define AP_FILTER_NUM_FILTERS 0 +#endif +#endif + +#ifndef AP_FILTER_ENABLED +#define AP_FILTER_ENABLED AP_FILTER_NUM_FILTERS > 0 +#endif diff --git a/libraries/Filter/AP_Filter_params.cpp b/libraries/Filter/AP_Filter_params.cpp new file mode 100644 index 0000000000..d13cb2477f --- /dev/null +++ b/libraries/Filter/AP_Filter_params.cpp @@ -0,0 +1,25 @@ +#include "AP_Filter.h" +#include +#include + +#if AP_FILTER_ENABLED && !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + +const AP_Param::GroupInfo AP_Filter_params::var_info[] = { + + // @Param: TYPE + // @DisplayName: Filter Type + // @Values: 0:Disable, 1:Notch Filter + // @Description: Filter Type + // @User: Standard + // @RebootRequired: True + AP_GROUPINFO_FLAGS("TYPE", 1, AP_Filter_params, _type, int8_t(AP_Filter::FilterType::FILTER_NONE), AP_PARAM_FLAG_ENABLE), + + AP_GROUPEND +}; + +AP_Filter_params::AP_Filter_params() +{ + AP_Param::setup_object_defaults(this, var_info); +} + +#endif // AP_FILTER_ENABLED && !APM_BUILD_TYPE(APM_BUILD_AP_Periph) diff --git a/libraries/Filter/AP_NotchFilter_params.cpp b/libraries/Filter/AP_NotchFilter_params.cpp new file mode 100644 index 0000000000..5927fef445 --- /dev/null +++ b/libraries/Filter/AP_NotchFilter_params.cpp @@ -0,0 +1,53 @@ +#include "AP_Filter.h" +#include +#include + +#if AP_FILTER_ENABLED && !APM_BUILD_TYPE(APM_BUILD_AP_Periph) + +const AP_Param::GroupInfo AP_NotchFilter_params::var_info[] = { + + // @Param: NOTCH_FREQ + // @DisplayName: Notch Filter center frequency + // @Description: Notch Filter center frequency in Hz. + // @Range: 10 495 + // @Units: Hz + // @User: Advanced + AP_GROUPINFO("NOTCH_FREQ", 1, AP_NotchFilter_params, _center_freq_hz, 0), + + // @Param: NOTCH_Q + // @DisplayName: Notch Filter quality factor + // @Description: Notch Filter quality factor given by the notch centre frequency divided by its bandwidth. + // @Range: 1 10 + // @User: Advanced + AP_GROUPINFO("NOTCH_Q", 2, AP_NotchFilter_params, _quality, 2), + + // @Param: NOTCH_ATT + // @DisplayName: Notch Filter attenuation + // @Description: Notch Filter attenuation in dB. + // @Range: 5 50 + // @Units: dB + // @User: Advanced + AP_GROUPINFO("NOTCH_ATT", 3, AP_NotchFilter_params, _attenuation_dB, 40), + + AP_GROUPEND +}; + +AP_NotchFilter_params::AP_NotchFilter_params() : AP_Filter(AP_Filter::FilterType::FILTER_NOTCH) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +bool AP_NotchFilter_params::setup_notch_filter(NotchFilterFloat& filter, float sample_rate) +{ + if (is_zero(_quality) || is_zero(_center_freq_hz) || is_zero(_attenuation_dB)) { + return false; + } + + if (!is_equal(sample_rate, filter.sample_freq_hz()) + || !is_equal(_center_freq_hz.get(), filter.center_freq_hz())) { + filter.init(sample_rate, _center_freq_hz, _center_freq_hz / _quality, _attenuation_dB); + } + return true; +} + +#endif // AP_FILTER_ENABLED && !APM_BUILD_TYPE(APM_BUILD_AP_Periph)