From d7756f7572cce12fc5244e1465c4c6429c010427 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 9 Jan 2021 21:04:58 +1100 Subject: [PATCH 1/3] Filter: added reset with value to LowPassFilter2p --- libraries/Filter/LowPassFilter2p.cpp | 17 +++++++++++++++++ libraries/Filter/LowPassFilter2p.h | 3 +++ .../LowPassFilter2p/LowPassFilter2p.cpp | 10 +++------- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp index 653c3a4a62..93f9bb86ef 100644 --- a/libraries/Filter/LowPassFilter2p.cpp +++ b/libraries/Filter/LowPassFilter2p.cpp @@ -17,6 +17,11 @@ T DigitalBiquadFilter::apply(const T &sample, const struct biquad_params &par return sample; } + if (!initialised) { + reset(sample, params); + initialised = true; + } + T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2; T output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2; @@ -29,6 +34,13 @@ T DigitalBiquadFilter::apply(const T &sample, const struct biquad_params &par template void DigitalBiquadFilter::reset() { _delay_element_1 = _delay_element_2 = T(); + initialised = false; +} + +template +void DigitalBiquadFilter::reset(const T &value, const struct biquad_params ¶ms) { + _delay_element_1 = _delay_element_2 = value * (1.0 / (1 + params.a1 + params.a2)); + initialised = true; } template @@ -99,6 +111,11 @@ void LowPassFilter2p::reset(void) { return _filter.reset(); } +template +void LowPassFilter2p::reset(const T &value) { + return _filter.reset(value, _params); +} + /* * Make an instances * Otherwise we have to move the constructor implementations to the header file :P diff --git a/libraries/Filter/LowPassFilter2p.h b/libraries/Filter/LowPassFilter2p.h index 65ed7c8ba4..5197f97579 100644 --- a/libraries/Filter/LowPassFilter2p.h +++ b/libraries/Filter/LowPassFilter2p.h @@ -39,11 +39,13 @@ class DigitalBiquadFilter { T apply(const T &sample, const struct biquad_params ¶ms); void reset(); + void reset(const T &value, const struct biquad_params ¶ms); static void compute_params(float sample_freq, float cutoff_freq, biquad_params &ret); private: T _delay_element_1; T _delay_element_2; + bool initialised; }; template @@ -59,6 +61,7 @@ class LowPassFilter2p { float get_sample_freq(void) const; T apply(const T &sample); void reset(void); + void reset(const T &value); protected: struct DigitalBiquadFilter::biquad_params _params; diff --git a/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp b/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp index 4347c703e3..bdbe5a7c26 100644 --- a/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp +++ b/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp @@ -26,16 +26,12 @@ void loop() for(int16_t i = 0; i < 300; i++ ) { // new data value - const float new_value = sinf((float)i * 2 * M_PI * 5 / 50.0f); // 5hz - - // output to user - hal.console->printf("applying: %6.4f", (double)new_value); - + const float new_value = 17 + sinf((float)i * 2 * M_PI * 5 / 50.0f); // 5hz // apply new value and retrieved filtered result const float filtered_value = low_pass_filter.apply(new_value); - // display results - hal.console->printf("\toutput: %6.4f\n", (double)filtered_value); + // output to user + hal.console->printf("applying: %6.4f -> %6.4f\n", (double)new_value, filtered_value); hal.scheduler->delay(10); } From 8b7142e700cbc86194683449106c6a4dfa1bbdb4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 26 Feb 2021 11:36:53 +1100 Subject: [PATCH 2/3] Filter: added a ComplementaryFilter library --- libraries/Filter/ComplementaryFilter.cpp | 90 ++++++++++++++++++++++++ libraries/Filter/ComplementaryFilter.h | 45 ++++++++++++ libraries/Filter/LowPassFilter2p.cpp | 1 + 3 files changed, 136 insertions(+) create mode 100644 libraries/Filter/ComplementaryFilter.cpp create mode 100644 libraries/Filter/ComplementaryFilter.h diff --git a/libraries/Filter/ComplementaryFilter.cpp b/libraries/Filter/ComplementaryFilter.cpp new file mode 100644 index 0000000000..ecfbc2f1f1 --- /dev/null +++ b/libraries/Filter/ComplementaryFilter.cpp @@ -0,0 +1,90 @@ +/* + This program 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 program 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 . + */ + +#include "ComplementaryFilter.h" + +/* + complementary filter. Used to combine a reading that is long term + stable but has high freq noise with another reading that has less + high frequency noise and poor long term stability. + */ + +/* + set crossover frequency between low frequency and high frequency + components + */ +void ComplementaryFilter::set_cutoff_frequency(float freq_hz) +{ + cutoff_freq = freq_hz; +} + +/* + reset the filter to initial values + */ +void ComplementaryFilter::reset() +{ + lp.reset(); + hp.reset(); +} + +/* + apply a low freqency and high freqency input to give a complementary + filter result where signal below the cutoff comes from the low_freq + input and above the cutoff from high freqency input. We take a + timestamp on each input as the input data may vary somewhat in + frequency + */ +float ComplementaryFilter::apply(float low_freq, float high_freq, uint32_t time_us) +{ + if (!is_positive(cutoff_freq)) { + reset(); + return low_freq; + } + const uint32_t dt_us = time_us - last_sample_us; + + if (dt_us > 1e6) { + // if we have not updated for 1s then assume we've had a + // sensor outage and reset + reset(); + } + last_sample_us = time_us; + + const float dt = MIN(dt_us * 1.0e-6, 1); + + // keep a low pass filter of the sample rate. Rapidly changing + // sample frequency in bi-quad filters leads to very nasty spikes + if (!is_positive(sample_freq)) { + sample_freq = 1.0/dt; + } else { + sample_freq = 0.99 * sample_freq + 0.01 * (1.0/dt); + } + + lp.compute_params(sample_freq, cutoff_freq, params); + + float hp_out = hp.apply(high_freq, params); + float lp_out = lp.apply(low_freq, params); + + out = (high_freq - hp_out) + lp_out; + + return out; +} + +/* + return the current value + */ +float ComplementaryFilter::get(void) +{ + return out; +} diff --git a/libraries/Filter/ComplementaryFilter.h b/libraries/Filter/ComplementaryFilter.h new file mode 100644 index 0000000000..637a5ff6d3 --- /dev/null +++ b/libraries/Filter/ComplementaryFilter.h @@ -0,0 +1,45 @@ +/* + This program 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 program 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 "LowPassFilter2p.h" + +/* + complementary filter. Used to combine a reading that is long term + stable but has high freq noise with another reading that has less + high frequency noise and poor long term stability. + */ + +class ComplementaryFilter +{ +public: + void set_cutoff_frequency(float freq_hz); + void reset(); + float apply(float low_freq, float high_freq, uint32_t time_us); + float get(void); + +private: + uint32_t last_sample_us; + float cutoff_freq; + float sample_freq; + // use 2-pole low pass filters to get a reasonably sharp cutoff + DigitalBiquadFilter::biquad_params params; + DigitalBiquadFilter lp, hp; + float out; +}; + + diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp index 93f9bb86ef..e443d83787 100644 --- a/libraries/Filter/LowPassFilter2p.cpp +++ b/libraries/Filter/LowPassFilter2p.cpp @@ -125,3 +125,4 @@ template class LowPassFilter2p; template class LowPassFilter2p; template class LowPassFilter2p; template class LowPassFilter2p; +template class DigitalBiquadFilter; From eb74a8037d794ddd1d0d444b68048ba3f70f9ce2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 26 Feb 2021 11:38:55 +1100 Subject: [PATCH 3/3] Copter: use a complementary filter for rangefinder data this should reduce the impact of noise in the rangefinder data. Filter cutoff frequency is low in loiter and higher otherwise --- ArduCopter/APM_Config.h | 3 ++- ArduCopter/Copter.h | 4 +++- ArduCopter/mode.h | 5 ++++- ArduCopter/sensors.cpp | 39 +++++++++++++++++++++++++++++++++++++-- 4 files changed, 46 insertions(+), 5 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 6554e098a5..25f5557250 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -5,7 +5,8 @@ #define ARMING_DELAY_SEC 3.0f -#define RANGEFINDER_WPNAV_FILT_HZ 3.0f +#define RANGEFINDER_WPNAV_FILT_MIN_HZ 0.05 +#define RANGEFINDER_WPNAV_FILT_MAX_HZ 0.5 #define LAND_DETECTOR_TRIGGER_SEC 0.5f diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 698d7c7d72..243e61c0b6 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -49,6 +49,7 @@ #include // AP Motors library #include // statistics library #include // Filter library +#include #include // needed for AHRS build #include // needed for AHRS build #include // ArduPilot Mega inertial navigation library @@ -281,7 +282,8 @@ class Copter : public AP_Vehicle { int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder float inertial_alt_cm; // inertial alt at time of last rangefinder sample uint32_t last_healthy_ms; - LowPassFilterFloat alt_cm_filt; // altitude filter + ComplementaryFilter alt_cm_filt; // altitude filter + float filter_hz; int16_t alt_cm_glitch_protected; // last glitch protected altitude int8_t glitch_count; // non-zero number indicates rangefinder is glitching uint32_t glitch_cleared_ms; // system time glitch cleared diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 087d54331c..e41440a6d8 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -366,6 +366,9 @@ class ModeAuto : public Mode { // for GCS_MAVLink to call: bool do_guided(const AP_Mission::Mission_Command& cmd); + // are we in a loiter? + bool in_loiter(void) const { return loiter_time != 0; } + AP_Mission mission{ FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &), FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &), @@ -1470,4 +1473,4 @@ class ModeAutorotate : public Mode { void warning_message(uint8_t message_n); //Handles output messages to the terminal }; -#endif \ No newline at end of file +#endif diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index fd1e2ab616..82dc6b7dc0 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -83,9 +83,44 @@ void Copter::read_rangefinder(void) if (rf_state.alt_healthy) { if (timed_out) { // reset filter if we haven't used it within the last second - rf_state.alt_cm_filt.reset(rf_state.alt_cm); + rf_state.alt_cm_filt.reset(); } else { - rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f); + /* + Apply a complementary filter to the rangefinder data, using the + inertial altitude for the high frequency component and the + rangefinder data for the low frequency component. + + When we are loitering shift the time constant of the + complementary filter towards a much lower frequency to filter + out even quite long period noise from the rangefinder. When not + loitering shift back to a shorter time period to handle changes + in terrain + */ + float target_filter_hz = RANGEFINDER_WPNAV_FILT_MAX_HZ; + if (control_mode == Mode::Number::AUTO && + mode_auto.mission.get_current_nav_cmd().id == MAV_CMD_NAV_WAYPOINT && + mode_auto.in_loiter()) { + // we are holding position at a waypoint, slew the time constant + target_filter_hz = RANGEFINDER_WPNAV_FILT_MIN_HZ; + } + + if (!is_equal(rangefinder_state.filter_hz, target_filter_hz)) { + rf_state.alt_cm_filt.set_cutoff_frequency(target_filter_hz); + rf_state.alt_cm_filt.reset(); + rangefinder_state.filter_hz = target_filter_hz; + } + + rf_state.alt_cm_filt.apply(rf_state.alt_cm, inertial_nav.get_altitude(), AP_HAL::micros()); + + // temporary debug log message to look at the + // performance of the complementary filter + AP::logger().Write("RF2", "TimeUS,LF,HF,Out,FHz", "Qffff", + AP_HAL::micros64(), + rf_state.alt_cm*0.01, + inertial_nav.get_altitude()*0.01, + rf_state.alt_cm_filt.get()*0.01, + rf_state.filter_hz); + } rf_state.last_healthy_ms = now; }