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

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
This commit is contained in:
Andy Piper 2023-10-20 13:31:27 +01:00 committed by Andrew Tridgell
parent 97a4eed232
commit e729c8ccfa
5 changed files with 340 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

155
libraries/Filter/AP_Filter.cpp Normal file
View File

@ -0,0 +1,155 @@
#include "AP_Filter.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_HAL/AP_HAL_Boards.h>
#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)

92
libraries/Filter/AP_Filter.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#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<AP_Filter::FilterType> _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

15
libraries/Filter/AP_Filter_config.h Normal file
View File

@ -0,0 +1,15 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#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

25
libraries/Filter/AP_Filter_params.cpp Normal file
View File

@ -0,0 +1,25 @@
#include "AP_Filter.h"
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#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)

53
libraries/Filter/AP_NotchFilter_params.cpp Normal file
View File

@ -0,0 +1,53 @@
#include "AP_Filter.h"
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#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)