PX4 Firmware
PX4 Autopilot Software http://px4.io
math::LowPassFilter2pVector3f Class Reference

#include <LowPassFilter2pVector3f.hpp>

Collaboration diagram for math::LowPassFilter2pVector3f:

Public Member Functions

 LowPassFilter2pVector3f (float sample_freq, float cutoff_freq)
 
void set_cutoff_frequency (float sample_freq, float cutoff_freq)
 
matrix::Vector3f apply (const matrix::Vector3f &sample)
 Add a new raw value to the filter. More...
 
float get_cutoff_freq () const
 
matrix::Vector3f reset (const matrix::Vector3f &sample)
 

Private Attributes

float _cutoff_freq {0.0f}
 
float _a1 {0.0f}
 
float _a2 {0.0f}
 
float _b0 {0.0f}
 
float _b1 {0.0f}
 
float _b2 {0.0f}
 
matrix::Vector3f _delay_element_1 {0.0f, 0.0f, 0.0f}
 
matrix::Vector3f _delay_element_2 {0.0f, 0.0f, 0.0f}
 

Detailed Description

Definition at line 44 of file LowPassFilter2pVector3f.hpp.

Constructor & Destructor Documentation

◆ LowPassFilter2pVector3f()

math::LowPassFilter2pVector3f::LowPassFilter2pVector3f ( float  sample_freq,
float  cutoff_freq 
)
inline

Definition at line 48 of file LowPassFilter2pVector3f.hpp.

References set_cutoff_frequency().

Here is the call graph for this function:

Member Function Documentation

◆ apply()

matrix::Vector3f math::LowPassFilter2pVector3f::apply ( const matrix::Vector3f sample)
inline

Add a new raw value to the filter.

Returns
retrieve the filtered result

Definition at line 62 of file LowPassFilter2pVector3f.hpp.

References _a1, _a2, _b0, _b1, _b2, _delay_element_1, and _delay_element_2.

Referenced by reset(), PX4Accelerometer::update(), and PX4Gyroscope::update().

Here is the caller graph for this function:

◆ get_cutoff_freq()

float math::LowPassFilter2pVector3f::get_cutoff_freq ( ) const
inline

Definition at line 75 of file LowPassFilter2pVector3f.hpp.

References _cutoff_freq, and reset().

Referenced by PX4Accelerometer::print_status(), PX4Gyroscope::print_status(), PX4Accelerometer::set_sample_rate(), and PX4Gyroscope::set_sample_rate().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ reset()

matrix::Vector3f math::LowPassFilter2pVector3f::reset ( const matrix::Vector3f sample)

Definition at line 75 of file LowPassFilter2pVector3f.cpp.

References _b0, _b1, _b2, _delay_element_1, _delay_element_2, and apply().

Referenced by get_cutoff_freq().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ set_cutoff_frequency()

void math::LowPassFilter2pVector3f::set_cutoff_frequency ( float  sample_freq,
float  cutoff_freq 
)

Definition at line 43 of file LowPassFilter2pVector3f.cpp.

References _a1, _a2, _b0, _b1, _b2, _cutoff_freq, _delay_element_1, _delay_element_2, f(), M_PI_F, and matrix::Matrix< Type, M, N >::zero().

Referenced by PX4Accelerometer::configure_filter(), PX4Gyroscope::configure_filter(), LowPassFilter2pVector3f(), PX4Accelerometer::set_sample_rate(), and PX4Gyroscope::set_sample_rate().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Data Documentation

◆ _a1

float math::LowPassFilter2pVector3f::_a1 {0.0f}
private

Definition at line 84 of file LowPassFilter2pVector3f.hpp.

Referenced by apply(), and set_cutoff_frequency().

◆ _a2

float math::LowPassFilter2pVector3f::_a2 {0.0f}
private

Definition at line 85 of file LowPassFilter2pVector3f.hpp.

Referenced by apply(), and set_cutoff_frequency().

◆ _b0

float math::LowPassFilter2pVector3f::_b0 {0.0f}
private

Definition at line 87 of file LowPassFilter2pVector3f.hpp.

Referenced by apply(), reset(), and set_cutoff_frequency().

◆ _b1

float math::LowPassFilter2pVector3f::_b1 {0.0f}
private

Definition at line 88 of file LowPassFilter2pVector3f.hpp.

Referenced by apply(), reset(), and set_cutoff_frequency().

◆ _b2

float math::LowPassFilter2pVector3f::_b2 {0.0f}
private

Definition at line 89 of file LowPassFilter2pVector3f.hpp.

Referenced by apply(), reset(), and set_cutoff_frequency().

◆ _cutoff_freq

float math::LowPassFilter2pVector3f::_cutoff_freq {0.0f}
private

Definition at line 82 of file LowPassFilter2pVector3f.hpp.

Referenced by get_cutoff_freq(), and set_cutoff_frequency().

◆ _delay_element_1

matrix::Vector3f math::LowPassFilter2pVector3f::_delay_element_1 {0.0f, 0.0f, 0.0f}
private

Definition at line 91 of file LowPassFilter2pVector3f.hpp.

Referenced by apply(), reset(), and set_cutoff_frequency().

◆ _delay_element_2

matrix::Vector3f math::LowPassFilter2pVector3f::_delay_element_2 {0.0f, 0.0f, 0.0f}
private

Definition at line 92 of file LowPassFilter2pVector3f.hpp.

Referenced by apply(), reset(), and set_cutoff_frequency().


The documentation for this class was generated from the following files: