42 #include <px4_platform_common/defines.h> 58 template<
class Type,
size_t M>
69 for (
size_t i = 0; i < M; i++) {
70 _state(i) = 0.0f / 0.0f;
76 for (
size_t i = 0; i < M; i++) {
77 if (!PX4_ISFINITE(getState()(i))) {
82 float b = 2 * float(
M_PI) * getFCut() * getDt();
83 float a = b / (1 + b);
84 setState(input * a + getState() * (1 - a));
matrix::Vector< Type, M > update(const matrix::Matrix< Type, M, 1 > &input)
matrix::Vector< Type, M > getState()
control::BlockParamFloat _fCut
void setState(const matrix::Vector< Type, M > &state)
matrix::Vector< Type, M > _state
BlockLowPassVector(SuperBlock *parent, const char *name)