mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
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:
parent
97a4eed232
commit
e729c8ccfa
155
libraries/Filter/AP_Filter.cpp
Normal file
155
libraries/Filter/AP_Filter.cpp
Normal 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
92
libraries/Filter/AP_Filter.h
Normal 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
15
libraries/Filter/AP_Filter_config.h
Normal 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
25
libraries/Filter/AP_Filter_params.cpp
Normal 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
53
libraries/Filter/AP_NotchFilter_params.cpp
Normal 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)
|
Loading…
Reference in New Issue
Block a user