diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index d1e0a8dc55..79843b12e2 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 7242bdac9d..e57f571482 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -63,6 +63,7 @@ #include // APM FIFO Buffer #include // APM relay #include +#include #include // needed for AHRS build #include // needed for AHRS build #include // ArduPilot Mega inertial navigation library @@ -249,6 +250,8 @@ class Copter : public AP_HAL::HAL::Callbacks { uint32_t last_healthy_ms; float terrain_height_filt_cm; // terrain height relative to origin, filtered int8_t glitch_count; + ComplementaryFilter alt_cm_filt; // altitude filter + float filter_hz; } rangefinder_state = { false, false, 0}; #if RPM_ENABLED == ENABLED diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 243c0f1e6c..73cb9c9264 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -305,6 +305,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; } + protected: const char *name() const override { return "AUTO"; } diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 35b537e115..14620856e3 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -53,7 +53,48 @@ void Copter::read_rangefinder(void) // Remove rangefinder body offset - converting NED to NEU rangefinder_alt_meas += (ahrs.get_rotation_body_to_ned()*rangefinder_pos_offset_cm).z; - float terrain_height_meas = inertial_nav.get_altitude() - rangefinder_alt_meas; + /* + 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 == 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(target_filter_hz,rangefinder_state.filter_hz)) { + // when changing target freq we use a reset to prevent a numerical error in the complementary filter + rangefinder_state.filter_hz = target_filter_hz; + rangefinder_state.alt_cm_filt.set_cutoff_frequency(rangefinder_state.filter_hz); + rangefinder_state.alt_cm_filt.reset(); + } + + // apply complementary filter + if (rangefinder_alt_meas_valid) { + rangefinder_state.alt_cm_filt.apply(rangefinder_alt_meas, inertial_nav.get_altitude(), AP_HAL::micros()); + } + const float rangefinder_alt_flt = rangefinder_state.alt_cm_filt.get(); + + // temporary debug log message to look at the + // performance of the complementary filter + DataFlash_Class::instance()->Log_Write("RF2", "TimeUS,LF,HF,Out,FHz", "Qffff", + AP_HAL::micros64(), + rangefinder_alt_meas*0.01, + inertial_nav.get_altitude()*0.01, + rangefinder_alt_flt*0.01, + rangefinder_state.filter_hz); + + float terrain_height_meas = inertial_nav.get_altitude() - rangefinder_alt_flt; // filter rangefinder for use by AC_WPNav uint32_t tnow_ms = AP_HAL::millis(); 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 653c3a4a62..e443d83787 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 @@ -108,3 +125,4 @@ template class LowPassFilter2p; template class LowPassFilter2p; template class LowPassFilter2p; template class LowPassFilter2p; +template class DigitalBiquadFilter; 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); }