forked from MaEtUgR/FlyBed
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
introduced a 30Hz lowpass filter for the gyroscope data
to make the numerical derivative term usable and hence the quad more stable
- Loading branch information
Showing
6 changed files
with
209 additions
and
29 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,44 @@ | ||
| #include "LowPassFilter.h" | ||
|
|
||
| void LowPassFilter::set_cutoff_frequency(float sample_freq, float cutoff_freq) | ||
| { | ||
| _cutoff_freq = cutoff_freq; | ||
| if (_cutoff_freq <= 0.0f) { | ||
| // no filtering | ||
| return; | ||
| } | ||
| float fr = sample_freq/_cutoff_freq; | ||
| float ohm = tan(M_PI_F/fr); | ||
| float c = 1.0f+2.0f*cos(M_PI_F/4.0f)*ohm + ohm*ohm; | ||
| _b0 = ohm*ohm/c; | ||
| _b1 = 2.0f*_b0; | ||
| _b2 = _b0; | ||
| _a1 = 2.0f*(ohm*ohm-1.0f)/c; | ||
| _a2 = (1.0f-2.0f*cos(M_PI_F/4.0f)*ohm+ohm*ohm)/c; | ||
| } | ||
|
|
||
| float LowPassFilter::apply(float sample) | ||
| { | ||
| if (_cutoff_freq <= 0.0f) { | ||
| // no filtering | ||
| return sample; | ||
| } | ||
| // do the filtering | ||
| float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; | ||
| if (isnan(delay_element_0) || isinf(delay_element_0)) { | ||
| // don't allow bad values to propagate via the filter | ||
| delay_element_0 = sample; | ||
| } | ||
| float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; | ||
|
|
||
| _delay_element_2 = _delay_element_1; | ||
| _delay_element_1 = delay_element_0; | ||
|
|
||
| // return the value. Should be no need to check limits | ||
| return output; | ||
| } | ||
|
|
||
| float LowPassFilter::reset(float sample) { | ||
| _delay_element_1 = _delay_element_2 = sample; | ||
| return apply(sample); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,90 @@ | ||
| /**************************************************************************** | ||
| * | ||
| * Copyright (C) 2012 PX4 Development Team. All rights reserved. | ||
| * | ||
| * Redistribution and use in source and binary forms, with or without | ||
| * modification, are permitted provided that the following conditions | ||
| * are met: | ||
| * | ||
| * 1. Redistributions of source code must retain the above copyright | ||
| * notice, this list of conditions and the following disclaimer. | ||
| * 2. Redistributions in binary form must reproduce the above copyright | ||
| * notice, this list of conditions and the following disclaimer in | ||
| * the documentation and/or other materials provided with the | ||
| * distribution. | ||
| * 3. Neither the name PX4 nor the names of its contributors may be | ||
| * used to endorse or promote products derived from this software | ||
| * without specific prior written permission. | ||
| * | ||
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS | ||
| * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED | ||
| * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| * POSSIBILITY OF SUCH DAMAGE. | ||
| * | ||
| ****************************************************************************/ | ||
|
|
||
| /// @file LowPassFilter.h | ||
| /// @brief A class to implement a second order low pass filter | ||
| /// Author: Leonard Hall <[email protected]> | ||
| /// Adapted for PX4 by Andrew Tridgell | ||
|
|
||
| #ifndef LOWPASSFILTER_H_ | ||
| #define LOWPASSFILTER_H_ | ||
|
|
||
| #include "mbed.h" | ||
| #define M_PI_F 3.14159265358979323846f | ||
|
|
||
| class LowPassFilter | ||
| { | ||
| public: | ||
| // constructor | ||
| LowPassFilter(float sample_freq, float cutoff_freq) { | ||
| // set initial parameters | ||
| set_cutoff_frequency(sample_freq, cutoff_freq); | ||
| _delay_element_1 = _delay_element_2 = 0; | ||
| } | ||
|
|
||
| /** | ||
| * Change filter parameters | ||
| */ | ||
| void set_cutoff_frequency(float sample_freq, float cutoff_freq); | ||
|
|
||
| /** | ||
| * Add a new raw value to the filter | ||
| * | ||
| * @return retrieve the filtered result | ||
| */ | ||
| float apply(float sample); | ||
|
|
||
| /** | ||
| * Return the cutoff frequency | ||
| */ | ||
| float get_cutoff_freq(void) const { | ||
| return _cutoff_freq; | ||
| } | ||
|
|
||
| /** | ||
| * Reset the filter state to this value | ||
| */ | ||
| float reset(float sample); | ||
|
|
||
| private: | ||
| float _cutoff_freq; | ||
| float _a1; | ||
| float _a2; | ||
| float _b0; | ||
| float _b1; | ||
| float _b2; | ||
| float _delay_element_1; // buffered sample -1 | ||
| float _delay_element_2; // buffered sample -2 | ||
| }; | ||
|
|
||
| #endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters