From 69ba69f30ca887198c6efd72cf0743996c4f21c2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 2 Jun 2017 22:25:43 -0400 Subject: [PATCH] controllib split blocks into separate files --- src/lib/controllib/BlockDelay.hpp | 115 ++++ .../{blocks.cpp => BlockDerivative.cpp} | 79 --- src/lib/controllib/BlockDerivative.hpp | 100 +++ src/lib/controllib/BlockHighPass.cpp | 57 ++ src/lib/controllib/BlockHighPass.hpp | 87 +++ src/lib/controllib/BlockIntegral.cpp | 55 ++ src/lib/controllib/BlockIntegral.hpp | 79 +++ src/lib/controllib/BlockIntegralTrap.cpp | 57 ++ src/lib/controllib/BlockIntegralTrap.hpp | 90 +++ src/lib/controllib/BlockLimit.cpp | 60 ++ src/lib/controllib/BlockLimit.hpp | 83 +++ src/lib/controllib/BlockLimitSym.cpp | 60 ++ src/lib/controllib/BlockLimitSym.hpp | 80 +++ src/lib/controllib/BlockLowPass.cpp | 60 ++ src/lib/controllib/BlockLowPass.hpp | 83 +++ src/lib/controllib/BlockLowPass2.cpp | 62 ++ src/lib/controllib/BlockLowPass2.hpp | 86 +++ src/lib/controllib/BlockLowPassVector.hpp | 97 +++ src/lib/controllib/BlockOutput.hpp | 89 +++ src/lib/controllib/BlockP.hpp | 81 +++ src/lib/controllib/BlockPD.hpp | 88 +++ src/lib/controllib/BlockPI.hpp | 88 +++ src/lib/controllib/BlockPID.hpp | 96 +++ src/lib/controllib/BlockRandGauss.hpp | 106 +++ src/lib/controllib/BlockRandUniform.hpp | 92 +++ src/lib/controllib/BlockStats.hpp | 102 +++ src/lib/controllib/CMakeLists.txt | 9 +- src/lib/controllib/blocks.hpp | 646 +----------------- 28 files changed, 2080 insertions(+), 707 deletions(-) create mode 100644 src/lib/controllib/BlockDelay.hpp rename src/lib/controllib/{blocks.cpp => BlockDerivative.cpp} (62%) create mode 100644 src/lib/controllib/BlockDerivative.hpp create mode 100644 src/lib/controllib/BlockHighPass.cpp create mode 100644 src/lib/controllib/BlockHighPass.hpp create mode 100644 src/lib/controllib/BlockIntegral.cpp create mode 100644 src/lib/controllib/BlockIntegral.hpp create mode 100644 src/lib/controllib/BlockIntegralTrap.cpp create mode 100644 src/lib/controllib/BlockIntegralTrap.hpp create mode 100644 src/lib/controllib/BlockLimit.cpp create mode 100644 src/lib/controllib/BlockLimit.hpp create mode 100644 src/lib/controllib/BlockLimitSym.cpp create mode 100644 src/lib/controllib/BlockLimitSym.hpp create mode 100644 src/lib/controllib/BlockLowPass.cpp create mode 100644 src/lib/controllib/BlockLowPass.hpp create mode 100644 src/lib/controllib/BlockLowPass2.cpp create mode 100644 src/lib/controllib/BlockLowPass2.hpp create mode 100644 src/lib/controllib/BlockLowPassVector.hpp create mode 100644 src/lib/controllib/BlockOutput.hpp create mode 100644 src/lib/controllib/BlockP.hpp create mode 100644 src/lib/controllib/BlockPD.hpp create mode 100644 src/lib/controllib/BlockPI.hpp create mode 100644 src/lib/controllib/BlockPID.hpp create mode 100644 src/lib/controllib/BlockRandGauss.hpp create mode 100644 src/lib/controllib/BlockRandUniform.hpp create mode 100644 src/lib/controllib/BlockStats.hpp diff --git a/src/lib/controllib/BlockDelay.hpp b/src/lib/controllib/BlockDelay.hpp new file mode 100644 index 0000000000..686aec5e07 --- /dev/null +++ b/src/lib/controllib/BlockDelay.hpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +template +class __EXPORT BlockDelay: public Block +{ +public: +// methods + BlockDelay(SuperBlock *parent, const char *name) : + Block(parent, name), + _h(), + _index(0), + _delay(-1) + { + }; + virtual ~BlockDelay() {}; + matrix::Matrix update(const matrix::Matrix &u) + { + // store current value + _h[_index] = u; + + // delay starts at zero, then increases to LEN + _delay += 1; + + if (_delay > (LEN - 1)) { + _delay = LEN - 1; + } + + // compute position of delayed value + int j = _index - _delay; + + if (j < 0) { + j += LEN; + } + + // increment storage position + _index += 1; + + if (_index > (LEN - 1)) { + _index = 0; + } + + // get delayed value + return _h[j]; + } + matrix::Matrix get(size_t delay) + { + int j = _index - delay; + + if (j < 0) { j += LEN; } + + return _h[j]; + } +private: +// attributes + matrix::Matrix _h[LEN]; + size_t _index; + int _delay; +}; + +} // namespace control diff --git a/src/lib/controllib/blocks.cpp b/src/lib/controllib/BlockDerivative.cpp similarity index 62% rename from src/lib/controllib/blocks.cpp rename to src/lib/controllib/BlockDerivative.cpp index 0e0c3a6ccc..5708af5c48 100644 --- a/src/lib/controllib/blocks.cpp +++ b/src/lib/controllib/BlockDerivative.cpp @@ -38,92 +38,13 @@ */ #include -#include #include #include "blocks.hpp" -#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; } - namespace control { -float BlockLimit::update(float input) -{ - if (input > getMax()) { - input = _max.get(); - - } else if (input < getMin()) { - input = getMin(); - } - - return input; -} - -float BlockLimitSym::update(float input) -{ - if (input > getMax()) { - input = _max.get(); - - } else if (input < -getMax()) { - input = -getMax(); - } - - return input; -} - -float BlockLowPass::update(float input) -{ - if (!PX4_ISFINITE(getState())) { - setState(input); - } - - float b = 2 * float(M_PI) * getFCut() * getDt(); - float a = b / (1 + b); - setState(a * input + (1 - a)*getState()); - return getState(); -} - - -float BlockHighPass::update(float input) -{ - float b = 2 * float(M_PI) * getFCut() * getDt(); - float a = 1 / (1 + b); - setY(a * (getY() + input - getU())); - setU(input); - return getY(); -} - -float BlockLowPass2::update(float input) -{ - if (!PX4_ISFINITE(getState())) { - setState(input); - } - - if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) { - _lp.set_cutoff_frequency(_fs, getFCutParam()); - } - - _state = _lp.apply(input); - return _state; -} - -float BlockIntegral::update(float input) -{ - // trapezoidal integration - setY(_limit.update(getY() + input * getDt())); - return getY(); -} - -float BlockIntegralTrap::update(float input) -{ - // trapezoidal integration - setY(_limit.update(getY() + - (getU() + input) / 2.0f * getDt())); - setU(input); - return getY(); -} - float BlockDerivative::update(float input) { float output; diff --git a/src/lib/controllib/BlockDerivative.hpp b/src/lib/controllib/BlockDerivative.hpp new file mode 100644 index 0000000000..793f67177c --- /dev/null +++ b/src/lib/controllib/BlockDerivative.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include "BlockLowPass.hpp" + +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A simple derivative approximation. + * This uses the previous and current input. + * This has a built in low pass filter. + * @see LowPass + */ +class __EXPORT BlockDerivative : public SuperBlock +{ +public: +// methods + BlockDerivative(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _u(0), + _initialized(false), + _lowPass(this, "LP") + {}; + virtual ~BlockDerivative() {}; + + /** + * Update the state and get current derivative + * + * This call updates the state and gets the current + * derivative. As the derivative is only valid + * on the second call to update, it will return + * no change (0) on the first. To get a closer + * estimate of the derivative on the first call, + * call setU() one time step before using the + * return value of update(). + * + * @param input the variable to calculate the derivative of + * @return the current derivative + */ + float update(float input); +// accessors + void setU(float u) {_u = u;} + float getU() {return _u;} + float getLP() {return _lowPass.getFCut();} + float getO() { return _lowPass.getState(); } +protected: +// attributes + float _u; /**< previous input */ + bool _initialized; + BlockLowPass _lowPass; /**< low pass filter */ +}; + +} // namespace control diff --git a/src/lib/controllib/BlockHighPass.cpp b/src/lib/controllib/BlockHighPass.cpp new file mode 100644 index 0000000000..9979996c3c --- /dev/null +++ b/src/lib/controllib/BlockHighPass.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 blocks.cpp + * + * Controller library code + */ + +#include +#include + +#include "blocks.hpp" + +namespace control +{ + +float BlockHighPass::update(float input) +{ + float b = 2 * float(M_PI) * getFCut() * getDt(); + float a = 1 / (1 + b); + setY(a * (getY() + input - getU())); + setU(input); + return getY(); +} + +} // namespace control diff --git a/src/lib/controllib/BlockHighPass.hpp b/src/lib/controllib/BlockHighPass.hpp new file mode 100644 index 0000000000..80763c8173 --- /dev/null +++ b/src/lib/controllib/BlockHighPass.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A high pass filter as described here: + * http://en.wikipedia.org/wiki/High-pass_filter. + */ +class __EXPORT BlockHighPass : public Block +{ +public: +// methods + BlockHighPass(SuperBlock *parent, const char *name) : + Block(parent, name), + _u(0), + _y(0), + _fCut(this, "") // only one parameter, no need to name + {}; + virtual ~BlockHighPass() {}; + float update(float input); +// accessors + float getU() {return _u;} + float getY() {return _y;} + float getFCut() {return _fCut.get();} + void setU(float u) {_u = u;} + void setY(float y) {_y = y;} +protected: +// attributes + float _u; /**< previous input */ + float _y; /**< previous output */ + control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */ +}; + +} // namespace control diff --git a/src/lib/controllib/BlockIntegral.cpp b/src/lib/controllib/BlockIntegral.cpp new file mode 100644 index 0000000000..3149640b77 --- /dev/null +++ b/src/lib/controllib/BlockIntegral.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 blocks.cpp + * + * Controller library code + */ + +#include +#include + +#include "blocks.hpp" + +namespace control +{ + +float BlockIntegral::update(float input) +{ + // trapezoidal integration + setY(_limit.update(getY() + input * getDt())); + return getY(); +} + +} // namespace control diff --git a/src/lib/controllib/BlockIntegral.hpp b/src/lib/controllib/BlockIntegral.hpp new file mode 100644 index 0000000000..f64f7791c5 --- /dev/null +++ b/src/lib/controllib/BlockIntegral.hpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include "BlockLimitSym.hpp" + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A rectangular integrator. + * A limiter is built into the class to bound the + * integral's internal state. This is important + * for windup protection. + * @see Limit + */ +class __EXPORT BlockIntegral: public SuperBlock +{ +public: +// methods + BlockIntegral(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _y(0), + _limit(this, "") {}; + virtual ~BlockIntegral() {}; + float update(float input); +// accessors + float getY() {return _y;} + float getMax() {return _limit.getMax();} + void setY(float y) {_y = y;} +protected: +// attributes + float _y; /**< previous output */ + BlockLimitSym _limit; /**< limiter */ +}; + +} // namespace control diff --git a/src/lib/controllib/BlockIntegralTrap.cpp b/src/lib/controllib/BlockIntegralTrap.cpp new file mode 100644 index 0000000000..af2fa24a43 --- /dev/null +++ b/src/lib/controllib/BlockIntegralTrap.cpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 blocks.cpp + * + * Controller library code + */ + +#include +#include + +#include "blocks.hpp" + +namespace control +{ + +float BlockIntegralTrap::update(float input) +{ + // trapezoidal integration + setY(_limit.update(getY() + + (getU() + input) / 2.0f * getDt())); + setU(input); + return getY(); +} + +} // namespace control diff --git a/src/lib/controllib/BlockIntegralTrap.hpp b/src/lib/controllib/BlockIntegralTrap.hpp new file mode 100644 index 0000000000..0dd6515245 --- /dev/null +++ b/src/lib/controllib/BlockIntegralTrap.hpp @@ -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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A trapezoidal integrator. + * http://en.wikipedia.org/wiki/Trapezoidal_rule + * A limiter is built into the class to bound the + * integral's internal state. This is important + * for windup protection. + * @see Limit + */ +class __EXPORT BlockIntegralTrap : public SuperBlock +{ +public: +// methods + BlockIntegralTrap(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _u(0), + _y(0), + _limit(this, "") {}; + virtual ~BlockIntegralTrap() {}; + float update(float input); +// accessors + float getU() {return _u;} + float getY() {return _y;} + float getMax() {return _limit.getMax();} + void setU(float u) {_u = u;} + void setY(float y) {_y = y;} +protected: +// attributes + float _u; /**< previous input */ + float _y; /**< previous output */ + BlockLimitSym _limit; /**< limiter */ +}; + +} // namespace control diff --git a/src/lib/controllib/BlockLimit.cpp b/src/lib/controllib/BlockLimit.cpp new file mode 100644 index 0000000000..9c5e918820 --- /dev/null +++ b/src/lib/controllib/BlockLimit.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 blocks.cpp + * + * Controller library code + */ + +#include +#include + +#include "blocks.hpp" + +namespace control +{ + +float BlockLimit::update(float input) +{ + if (input > getMax()) { + input = _max.get(); + + } else if (input < getMin()) { + input = getMin(); + } + + return input; +} + +} // namespace control diff --git a/src/lib/controllib/BlockLimit.hpp b/src/lib/controllib/BlockLimit.hpp new file mode 100644 index 0000000000..992611ead4 --- /dev/null +++ b/src/lib/controllib/BlockLimit.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A limiter/ saturation. + * The output of update is the input, bounded + * by min/max. + */ +class __EXPORT BlockLimit : public Block +{ +public: +// methods + BlockLimit(SuperBlock *parent, const char *name) : + Block(parent, name), + _min(this, "MIN"), + _max(this, "MAX") + {}; + virtual ~BlockLimit() {}; + float update(float input); +// accessors + float getMin() { return _min.get(); } + float getMax() { return _max.get(); } +protected: +// attributes + control::BlockParamFloat _min; + control::BlockParamFloat _max; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockLimitSym.cpp b/src/lib/controllib/BlockLimitSym.cpp new file mode 100644 index 0000000000..5d6686515c --- /dev/null +++ b/src/lib/controllib/BlockLimitSym.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 blocks.cpp + * + * Controller library code + */ + +#include +#include + +#include "blocks.hpp" + +namespace control +{ + +float BlockLimitSym::update(float input) +{ + if (input > getMax()) { + input = _max.get(); + + } else if (input < -getMax()) { + input = -getMax(); + } + + return input; +} + +} // namespace control diff --git a/src/lib/controllib/BlockLimitSym.hpp b/src/lib/controllib/BlockLimitSym.hpp new file mode 100644 index 0000000000..e452a46942 --- /dev/null +++ b/src/lib/controllib/BlockLimitSym.hpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A symmetric limiter/ saturation. + * Same as limiter but with only a max, is used for + * upper limit of +max, and lower limit of -max + */ +class __EXPORT BlockLimitSym : public Block +{ +public: +// methods + BlockLimitSym(SuperBlock *parent, const char *name) : + Block(parent, name), + _max(this, "MAX") + {}; + virtual ~BlockLimitSym() {}; + float update(float input); +// accessors + float getMax() { return _max.get(); } +protected: +// attributes + control::BlockParamFloat _max; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockLowPass.cpp b/src/lib/controllib/BlockLowPass.cpp new file mode 100644 index 0000000000..b3551289c1 --- /dev/null +++ b/src/lib/controllib/BlockLowPass.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 blocks.cpp + * + * Controller library code + */ + +#include +#include + +#include "blocks.hpp" + +namespace control +{ + +float BlockLowPass::update(float input) +{ + if (!PX4_ISFINITE(getState())) { + setState(input); + } + + float b = 2 * float(M_PI) * getFCut() * getDt(); + float a = b / (1 + b); + setState(a * input + (1 - a)*getState()); + return getState(); +} + +} // namespace control diff --git a/src/lib/controllib/BlockLowPass.hpp b/src/lib/controllib/BlockLowPass.hpp new file mode 100644 index 0000000000..fee7467fa3 --- /dev/null +++ b/src/lib/controllib/BlockLowPass.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A low pass filter as described here: + * http://en.wikipedia.org/wiki/Low-pass_filter. + */ +class __EXPORT BlockLowPass : public Block +{ +public: +// methods + BlockLowPass(SuperBlock *parent, const char *name) : + Block(parent, name), + _state(0.0f / 0.0f /* initialize to invalid val, force into is_finite() check on first call */), + _fCut(this, "") // only one parameter, no need to name + {}; + virtual ~BlockLowPass() {}; + float update(float input); +// accessors + float getState() { return _state; } + float getFCut() { return _fCut.get(); } + void setState(float state) { _state = state; } +protected: +// attributes + float _state; + control::BlockParamFloat _fCut; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockLowPass2.cpp b/src/lib/controllib/BlockLowPass2.cpp new file mode 100644 index 0000000000..c9094e60ec --- /dev/null +++ b/src/lib/controllib/BlockLowPass2.cpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 blocks.cpp + * + * Controller library code + */ + +#include +#include + +#include "blocks.hpp" + +namespace control +{ + +float BlockLowPass2::update(float input) +{ + if (!PX4_ISFINITE(getState())) { + setState(input); + } + + if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) { + _lp.set_cutoff_frequency(_fs, getFCutParam()); + } + + _state = _lp.apply(input); + return _state; +} + +} // namespace control diff --git a/src/lib/controllib/BlockLowPass2.hpp b/src/lib/controllib/BlockLowPass2.hpp new file mode 100644 index 0000000000..bc215b5de6 --- /dev/null +++ b/src/lib/controllib/BlockLowPass2.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter + */ +class __EXPORT BlockLowPass2 : public Block +{ +public: +// methods + BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) : + Block(parent, name), + _state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */), + _fCut(this, ""), // only one parameter, no need to name + _fs(sample_freq), + _lp(_fs, _fCut.get()) + {}; + virtual ~BlockLowPass2() {}; + float update(float input); +// accessors + float getState() { return _state; } + float getFCutParam() { return _fCut.get(); } + void setState(float state) { _state = _lp.reset(state); } +protected: +// attributes + float _state; + control::BlockParamFloat _fCut; + float _fs; + math::LowPassFilter2p _lp; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockLowPassVector.hpp b/src/lib/controllib/BlockLowPassVector.hpp new file mode 100644 index 0000000000..fb5f0ba7ca --- /dev/null +++ b/src/lib/controllib/BlockLowPassVector.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +template +class __EXPORT BlockLowPassVector: public Block +{ +public: +// methods + BlockLowPassVector(SuperBlock *parent, + const char *name) : + Block(parent, name), + _state(), + _fCut(this, "") // only one parameter, no need to name + { + for (int i = 0; i < M; i++) { + _state(i) = 0.0f / 0.0f; + } + }; + virtual ~BlockLowPassVector() {}; + matrix::Vector update(const matrix::Matrix &input) + { + for (int i = 0; i < M; i++) { + if (!PX4_ISFINITE(getState()(i))) { + setState(input); + } + } + + float b = 2 * float(M_PI) * getFCut() * getDt(); + float a = b / (1 + b); + setState(input * a + getState() * (1 - a)); + return getState(); + } +// accessors + matrix::Vector getState() { return _state; } + float getFCut() { return _fCut.get(); } + void setState(const matrix::Vector &state) { _state = state; } +private: +// attributes + matrix::Vector _state; + control::BlockParamFloat _fCut; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockOutput.hpp b/src/lib/controllib/BlockOutput.hpp new file mode 100644 index 0000000000..1feaa8ec88 --- /dev/null +++ b/src/lib/controllib/BlockOutput.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ +/** + * An output trim/ saturation block + */ +class __EXPORT BlockOutput: public SuperBlock +{ +public: +// methods + BlockOutput(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _trim(this, "TRIM"), + _limit(this, ""), + _val(0) + { + update(0); + }; + virtual ~BlockOutput() {}; + void update(float input) + { + _val = _limit.update(input + getTrim()); + } +// accessors + float getMin() { return _limit.getMin(); } + float getMax() { return _limit.getMax(); } + float getTrim() { return _trim.get(); } + float get() { return _val; } +private: +// attributes + control::BlockParamFloat _trim; + BlockLimit _limit; + float _val; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockP.hpp b/src/lib/controllib/BlockP.hpp new file mode 100644 index 0000000000..e751947a84 --- /dev/null +++ b/src/lib/controllib/BlockP.hpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A proportional controller. + * @link http://en.wikipedia.org/wiki/PID_controller + */ +class __EXPORT BlockP: public Block +{ +public: +// methods + BlockP(SuperBlock *parent, const char *name) : + Block(parent, name), + _kP(this, "") // only one param, no need to name + {}; + virtual ~BlockP() {}; + float update(float input) + { + return getKP() * input; + } +// accessors + float getKP() { return _kP.get(); } +protected: + control::BlockParamFloat _kP; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockPD.hpp b/src/lib/controllib/BlockPD.hpp new file mode 100644 index 0000000000..2992af37dd --- /dev/null +++ b/src/lib/controllib/BlockPD.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A proportional-derivative controller. + * @link http://en.wikipedia.org/wiki/PID_controller + */ +class __EXPORT BlockPD: public SuperBlock +{ +public: +// methods + BlockPD(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _derivative(this, "D"), + _kP(this, "P"), + _kD(this, "D") + {}; + virtual ~BlockPD() {}; + float update(float input) + { + return getKP() * input + + getKD() * getDerivative().update(input); + } +// accessors + float getKP() { return _kP.get(); } + float getKD() { return _kD.get(); } + BlockDerivative &getDerivative() { return _derivative; } +private: + BlockDerivative _derivative; + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockPI.hpp b/src/lib/controllib/BlockPI.hpp new file mode 100644 index 0000000000..e352d0ffcf --- /dev/null +++ b/src/lib/controllib/BlockPI.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A proportional-integral controller. + * @link http://en.wikipedia.org/wiki/PID_controller + */ +class __EXPORT BlockPI: public SuperBlock +{ +public: +// methods + BlockPI(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _integral(this, "I"), + _kP(this, "P"), + _kI(this, "I") + {}; + virtual ~BlockPI() {}; + float update(float input) + { + return getKP() * input + + getKI() * getIntegral().update(input); + } +// accessors + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + BlockIntegral &getIntegral() { return _integral; } +private: + BlockIntegral _integral; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockPID.hpp b/src/lib/controllib/BlockPID.hpp new file mode 100644 index 0000000000..4c26796866 --- /dev/null +++ b/src/lib/controllib/BlockPID.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A proportional-integral-derivative controller. + * @link http://en.wikipedia.org/wiki/PID_controller + */ +class __EXPORT BlockPID: public SuperBlock +{ +public: +// methods + BlockPID(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _integral(this, "I"), + _derivative(this, "D"), + _kP(this, "P"), + _kI(this, "I"), + _kD(this, "D") + {}; + virtual ~BlockPID() {}; + float update(float input) + { + return getKP() * input + + getKI() * getIntegral().update(input) + + getKD() * getDerivative().update(input); + } +// accessors + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + float getKD() { return _kD.get(); } + BlockIntegral &getIntegral() { return _integral; } + BlockDerivative &getDerivative() { return _derivative; } +private: +// attributes + BlockIntegral _integral; + BlockDerivative _derivative; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; + control::BlockParamFloat _kD; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockRandGauss.hpp b/src/lib/controllib/BlockRandGauss.hpp new file mode 100644 index 0000000000..f38764a6bc --- /dev/null +++ b/src/lib/controllib/BlockRandGauss.hpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +class __EXPORT BlockRandGauss: public Block +{ +public: +// methods + BlockRandGauss(SuperBlock *parent, + const char *name) : + Block(parent, name), + _mean(this, "MEAN"), + _stdDev(this, "DEV") + { + // seed should be initialized somewhere + // in main program for all calls to rand + // XXX currently in nuttx if you seed to 0, rand breaks + }; + virtual ~BlockRandGauss() {}; + float update() + { + static float V1, V2, S; + static int phase = 0; + float X; + + if (phase == 0) { + do { + float U1 = (float)rand() / RAND_MAX; + float U2 = (float)rand() / RAND_MAX; + V1 = 2 * U1 - 1; + V2 = 2 * U2 - 1; + S = V1 * V1 + V2 * V2; + } while (S >= 1 || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2 * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2 * float(logf(S)) / S)); + } + + phase = 1 - phase; + return X * getStdDev() + getMean(); + } +// accessors + float getMean() { return _mean.get(); } + float getStdDev() { return _stdDev.get(); } +private: +// attributes + control::BlockParamFloat _mean; + control::BlockParamFloat _stdDev; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockRandUniform.hpp b/src/lib/controllib/BlockRandUniform.hpp new file mode 100644 index 0000000000..cbb0603776 --- /dev/null +++ b/src/lib/controllib/BlockRandUniform.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ + +/** + * A uniform random number generator + */ +class __EXPORT BlockRandUniform: public Block +{ +public: +// methods + BlockRandUniform(SuperBlock *parent, + const char *name) : + Block(parent, name), + _min(this, "MIN"), + _max(this, "MAX") + { + // seed should be initialized somewhere + // in main program for all calls to rand + // XXX currently in nuttx if you seed to 0, rand breaks + }; + virtual ~BlockRandUniform() {}; + float update() + { + static float rand_max = RAND_MAX; + float rand_val = rand(); + float bounds = getMax() - getMin(); + return getMin() + (rand_val * bounds) / rand_max; + } +// accessors + float getMin() { return _min.get(); } + float getMax() { return _max.get(); } +private: +// attributes + control::BlockParamFloat _min; + control::BlockParamFloat _max; +}; + +} // namespace control diff --git a/src/lib/controllib/BlockStats.hpp b/src/lib/controllib/BlockStats.hpp new file mode 100644 index 0000000000..e82be2a33d --- /dev/null +++ b/src/lib/controllib/BlockStats.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 blocks.h + * + * Controller library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +#include "matrix/math.hpp" + +namespace control +{ +template +class __EXPORT BlockStats: public Block +{ + +public: +// methods + BlockStats(SuperBlock *parent, + const char *name) : + Block(parent, name), + _sum(), + _sumSq(), + _count(0) + { + }; + virtual ~BlockStats() {}; + void update(const matrix::Vector &u) + { + _sum += u; + _sumSq += u.emult(u); + _count += 1; + } + void reset() + { + _sum.setZero(); + _sumSq.setZero(); + _count = 0; + } +// accessors + size_t getCount() { return _count; } + matrix::Vector getMean() { return _sum / _count; } + matrix::Vector getVar() + { + return (_sumSq - _sum.emult(_sum) / _count) / _count; + } + matrix::Vector getStdDev() + { + return getVar().pow(0.5); + } +private: +// attributes + matrix::Vector _sum; + matrix::Vector _sumSq; + size_t _count; +}; + +} // namespace control diff --git a/src/lib/controllib/CMakeLists.txt b/src/lib/controllib/CMakeLists.txt index 9d37781032..9d33534c2b 100644 --- a/src/lib/controllib/CMakeLists.txt +++ b/src/lib/controllib/CMakeLists.txt @@ -34,9 +34,16 @@ px4_add_module( MODULE lib__controllib COMPILE_FLAGS SRCS - blocks.cpp block/Block.cpp block/BlockParam.cpp + BlockDerivative.cpp + BlockHighPass.cpp + BlockIntegral.cpp + BlockIntegralTrap.cpp + BlockLimit.cpp + BlockLimitSym.cpp + BlockLowPass.cpp + BlockLowPass2.cpp DEPENDS platforms__common ) diff --git a/src/lib/controllib/blocks.hpp b/src/lib/controllib/blocks.hpp index 4f87fbd519..6ba49c0985 100644 --- a/src/lib/controllib/blocks.hpp +++ b/src/lib/controllib/blocks.hpp @@ -32,636 +32,28 @@ ****************************************************************************/ /** - * @file blocks.h + * @file blocks.hpp * * Controller library code */ #pragma once -#include -#include -#include -#include -#include -#include -#include - -#include "block/Block.hpp" -#include "block/BlockParam.hpp" - -#include "matrix/math.hpp" - -namespace control -{ - -/** - * A limiter/ saturation. - * The output of update is the input, bounded - * by min/max. - */ -class __EXPORT BlockLimit : public Block -{ -public: -// methods - BlockLimit(SuperBlock *parent, const char *name) : - Block(parent, name), - _min(this, "MIN"), - _max(this, "MAX") - {}; - virtual ~BlockLimit() {}; - float update(float input); -// accessors - float getMin() { return _min.get(); } - float getMax() { return _max.get(); } -protected: -// attributes - control::BlockParamFloat _min; - control::BlockParamFloat _max; -}; - -/** - * A symmetric limiter/ saturation. - * Same as limiter but with only a max, is used for - * upper limit of +max, and lower limit of -max - */ -class __EXPORT BlockLimitSym : public Block -{ -public: -// methods - BlockLimitSym(SuperBlock *parent, const char *name) : - Block(parent, name), - _max(this, "MAX") - {}; - virtual ~BlockLimitSym() {}; - float update(float input); -// accessors - float getMax() { return _max.get(); } -protected: -// attributes - control::BlockParamFloat _max; -}; - -/** - * A low pass filter as described here: - * http://en.wikipedia.org/wiki/Low-pass_filter. - */ -class __EXPORT BlockLowPass : public Block -{ -public: -// methods - BlockLowPass(SuperBlock *parent, const char *name) : - Block(parent, name), - _state(0.0f / 0.0f /* initialize to invalid val, force into is_finite() check on first call */), - _fCut(this, "") // only one parameter, no need to name - {}; - virtual ~BlockLowPass() {}; - float update(float input); -// accessors - float getState() { return _state; } - float getFCut() { return _fCut.get(); } - void setState(float state) { _state = state; } -protected: -// attributes - float _state; - control::BlockParamFloat _fCut; -}; - -template -class __EXPORT BlockLowPassVector: public Block -{ -public: -// methods - BlockLowPassVector(SuperBlock *parent, - const char *name) : - Block(parent, name), - _state(), - _fCut(this, "") // only one parameter, no need to name - { - for (int i = 0; i < M; i++) { - _state(i) = 0.0f / 0.0f; - } - }; - virtual ~BlockLowPassVector() {}; - matrix::Vector update(const matrix::Matrix &input) - { - for (int i = 0; i < M; i++) { - if (!PX4_ISFINITE(getState()(i))) { - setState(input); - } - } - - float b = 2 * float(M_PI) * getFCut() * getDt(); - float a = b / (1 + b); - setState(input * a + getState() * (1 - a)); - return getState(); - } -// accessors - matrix::Vector getState() { return _state; } - float getFCut() { return _fCut.get(); } - void setState(const matrix::Vector &state) { _state = state; } -private: -// attributes - matrix::Vector _state; - control::BlockParamFloat _fCut; -}; - -/** - * A high pass filter as described here: - * http://en.wikipedia.org/wiki/High-pass_filter. - */ -class __EXPORT BlockHighPass : public Block -{ -public: -// methods - BlockHighPass(SuperBlock *parent, const char *name) : - Block(parent, name), - _u(0), - _y(0), - _fCut(this, "") // only one parameter, no need to name - {}; - virtual ~BlockHighPass() {}; - float update(float input); -// accessors - float getU() {return _u;} - float getY() {return _y;} - float getFCut() {return _fCut.get();} - void setU(float u) {_u = u;} - void setY(float y) {_y = y;} -protected: -// attributes - float _u; /**< previous input */ - float _y; /**< previous output */ - control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */ -}; - -/** - * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter - */ -class __EXPORT BlockLowPass2 : public Block -{ -public: -// methods - BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) : - Block(parent, name), - _state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */), - _fCut(this, ""), // only one parameter, no need to name - _fs(sample_freq), - _lp(_fs, _fCut.get()) - {}; - virtual ~BlockLowPass2() {}; - float update(float input); -// accessors - float getState() { return _state; } - float getFCutParam() { return _fCut.get(); } - void setState(float state) { _state = _lp.reset(state); } -protected: -// attributes - float _state; - control::BlockParamFloat _fCut; - float _fs; - math::LowPassFilter2p _lp; -}; - -/** - * A rectangular integrator. - * A limiter is built into the class to bound the - * integral's internal state. This is important - * for windup protection. - * @see Limit - */ -class __EXPORT BlockIntegral: public SuperBlock -{ -public: -// methods - BlockIntegral(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _y(0), - _limit(this, "") {}; - virtual ~BlockIntegral() {}; - float update(float input); -// accessors - float getY() {return _y;} - float getMax() {return _limit.getMax();} - void setY(float y) {_y = y;} -protected: -// attributes - float _y; /**< previous output */ - BlockLimitSym _limit; /**< limiter */ -}; - -/** - * A trapezoidal integrator. - * http://en.wikipedia.org/wiki/Trapezoidal_rule - * A limiter is built into the class to bound the - * integral's internal state. This is important - * for windup protection. - * @see Limit - */ -class __EXPORT BlockIntegralTrap : public SuperBlock -{ -public: -// methods - BlockIntegralTrap(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _u(0), - _y(0), - _limit(this, "") {}; - virtual ~BlockIntegralTrap() {}; - float update(float input); -// accessors - float getU() {return _u;} - float getY() {return _y;} - float getMax() {return _limit.getMax();} - void setU(float u) {_u = u;} - void setY(float y) {_y = y;} -protected: -// attributes - float _u; /**< previous input */ - float _y; /**< previous output */ - BlockLimitSym _limit; /**< limiter */ -}; - -/** - * A simple derivative approximation. - * This uses the previous and current input. - * This has a built in low pass filter. - * @see LowPass - */ -class __EXPORT BlockDerivative : public SuperBlock -{ -public: -// methods - BlockDerivative(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _u(0), - _initialized(false), - _lowPass(this, "LP") - {}; - virtual ~BlockDerivative() {}; - - /** - * Update the state and get current derivative - * - * This call updates the state and gets the current - * derivative. As the derivative is only valid - * on the second call to update, it will return - * no change (0) on the first. To get a closer - * estimate of the derivative on the first call, - * call setU() one time step before using the - * return value of update(). - * - * @param input the variable to calculate the derivative of - * @return the current derivative - */ - float update(float input); -// accessors - void setU(float u) {_u = u;} - float getU() {return _u;} - float getLP() {return _lowPass.getFCut();} - float getO() { return _lowPass.getState(); } -protected: -// attributes - float _u; /**< previous input */ - bool _initialized; - BlockLowPass _lowPass; /**< low pass filter */ -}; - -/** - * A proportional controller. - * @link http://en.wikipedia.org/wiki/PID_controller - */ -class __EXPORT BlockP: public Block -{ -public: -// methods - BlockP(SuperBlock *parent, const char *name) : - Block(parent, name), - _kP(this, "") // only one param, no need to name - {}; - virtual ~BlockP() {}; - float update(float input) - { - return getKP() * input; - } -// accessors - float getKP() { return _kP.get(); } -protected: - control::BlockParamFloat _kP; -}; - -/** - * A proportional-integral controller. - * @link http://en.wikipedia.org/wiki/PID_controller - */ -class __EXPORT BlockPI: public SuperBlock -{ -public: -// methods - BlockPI(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _integral(this, "I"), - _kP(this, "P"), - _kI(this, "I") - {}; - virtual ~BlockPI() {}; - float update(float input) - { - return getKP() * input + - getKI() * getIntegral().update(input); - } -// accessors - float getKP() { return _kP.get(); } - float getKI() { return _kI.get(); } - BlockIntegral &getIntegral() { return _integral; } -private: - BlockIntegral _integral; - control::BlockParamFloat _kP; - control::BlockParamFloat _kI; -}; - -/** - * A proportional-derivative controller. - * @link http://en.wikipedia.org/wiki/PID_controller - */ -class __EXPORT BlockPD: public SuperBlock -{ -public: -// methods - BlockPD(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _derivative(this, "D"), - _kP(this, "P"), - _kD(this, "D") - {}; - virtual ~BlockPD() {}; - float update(float input) - { - return getKP() * input + - getKD() * getDerivative().update(input); - } -// accessors - float getKP() { return _kP.get(); } - float getKD() { return _kD.get(); } - BlockDerivative &getDerivative() { return _derivative; } -private: - BlockDerivative _derivative; - control::BlockParamFloat _kP; - control::BlockParamFloat _kD; -}; - -/** - * A proportional-integral-derivative controller. - * @link http://en.wikipedia.org/wiki/PID_controller - */ -class __EXPORT BlockPID: public SuperBlock -{ -public: -// methods - BlockPID(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _integral(this, "I"), - _derivative(this, "D"), - _kP(this, "P"), - _kI(this, "I"), - _kD(this, "D") - {}; - virtual ~BlockPID() {}; - float update(float input) - { - return getKP() * input + - getKI() * getIntegral().update(input) + - getKD() * getDerivative().update(input); - } -// accessors - float getKP() { return _kP.get(); } - float getKI() { return _kI.get(); } - float getKD() { return _kD.get(); } - BlockIntegral &getIntegral() { return _integral; } - BlockDerivative &getDerivative() { return _derivative; } -private: -// attributes - BlockIntegral _integral; - BlockDerivative _derivative; - control::BlockParamFloat _kP; - control::BlockParamFloat _kI; - control::BlockParamFloat _kD; -}; - -/** - * An output trim/ saturation block - */ -class __EXPORT BlockOutput: public SuperBlock -{ -public: -// methods - BlockOutput(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _trim(this, "TRIM"), - _limit(this, ""), - _val(0) - { - update(0); - }; - virtual ~BlockOutput() {}; - void update(float input) - { - _val = _limit.update(input + getTrim()); - } -// accessors - float getMin() { return _limit.getMin(); } - float getMax() { return _limit.getMax(); } - float getTrim() { return _trim.get(); } - float get() { return _val; } -private: -// attributes - control::BlockParamFloat _trim; - BlockLimit _limit; - float _val; -}; - -/** - * A uniform random number generator - */ -class __EXPORT BlockRandUniform: public Block -{ -public: -// methods - BlockRandUniform(SuperBlock *parent, - const char *name) : - Block(parent, name), - _min(this, "MIN"), - _max(this, "MAX") - { - // seed should be initialized somewhere - // in main program for all calls to rand - // XXX currently in nuttx if you seed to 0, rand breaks - }; - virtual ~BlockRandUniform() {}; - float update() - { - static float rand_max = RAND_MAX; - float rand_val = rand(); - float bounds = getMax() - getMin(); - return getMin() + (rand_val * bounds) / rand_max; - } -// accessors - float getMin() { return _min.get(); } - float getMax() { return _max.get(); } -private: -// attributes - control::BlockParamFloat _min; - control::BlockParamFloat _max; -}; - -class __EXPORT BlockRandGauss: public Block -{ -public: -// methods - BlockRandGauss(SuperBlock *parent, - const char *name) : - Block(parent, name), - _mean(this, "MEAN"), - _stdDev(this, "DEV") - { - // seed should be initialized somewhere - // in main program for all calls to rand - // XXX currently in nuttx if you seed to 0, rand breaks - }; - virtual ~BlockRandGauss() {}; - float update() - { - static float V1, V2, S; - static int phase = 0; - float X; - - if (phase == 0) { - do { - float U1 = (float)rand() / RAND_MAX; - float U2 = (float)rand() / RAND_MAX; - V1 = 2 * U1 - 1; - V2 = 2 * U2 - 1; - S = V1 * V1 + V2 * V2; - } while (S >= 1 || fabsf(S) < 1e-8f); - - X = V1 * float(sqrtf(-2 * float(logf(S)) / S)); - - } else { - X = V2 * float(sqrtf(-2 * float(logf(S)) / S)); - } - - phase = 1 - phase; - return X * getStdDev() + getMean(); - } -// accessors - float getMean() { return _mean.get(); } - float getStdDev() { return _stdDev.get(); } -private: -// attributes - control::BlockParamFloat _mean; - control::BlockParamFloat _stdDev; -}; - -template -class __EXPORT BlockStats: public Block -{ -public: -// methods - BlockStats(SuperBlock *parent, - const char *name) : - Block(parent, name), - _sum(), - _sumSq(), - _count(0) - { - }; - virtual ~BlockStats() {}; - void update(const matrix::Vector &u) - { - _sum += u; - _sumSq += u.emult(u); - _count += 1; - } - void reset() - { - _sum.setZero(); - _sumSq.setZero(); - _count = 0; - } -// accessors - size_t getCount() { return _count; } - matrix::Vector getMean() { return _sum / _count; } - matrix::Vector getVar() - { - return (_sumSq - _sum.emult(_sum) / _count) / _count; - } - matrix::Vector getStdDev() - { - return getVar().pow(0.5); - } -private: -// attributes - matrix::Vector _sum; - matrix::Vector _sumSq; - size_t _count; -}; - -template -class __EXPORT BlockDelay: public Block -{ -public: -// methods - BlockDelay(SuperBlock *parent, - const char *name) : - Block(parent, name), - _h(), - _index(0), - _delay(-1) - { - }; - virtual ~BlockDelay() {}; - matrix::Matrix update(const matrix::Matrix &u) - { - // store current value - _h[_index] = u; - - // delay starts at zero, then increases to LEN - _delay += 1; - - if (_delay > (LEN - 1)) { - _delay = LEN - 1; - } - - // compute position of delayed value - int j = _index - _delay; - - if (j < 0) { - j += LEN; - } - - // increment storage position - _index += 1; - - if (_index > (LEN - 1)) { - _index = 0; - } - - // get delayed value - return _h[j]; - } - matrix::Matrix get(size_t delay) - { - int j = _index - delay; - - if (j < 0) { j += LEN; } - - return _h[j]; - } -private: -// attributes - matrix::Matrix _h[LEN]; - size_t _index; - int _delay; -}; - -} // namespace control +#include "BlockDelay.hpp" +#include "BlockDerivative.hpp" +#include "BlockHighPass.hpp" +#include "BlockIntegral.hpp" +#include "BlockIntegralTrap.hpp" +#include "BlockLimit.hpp" +#include "BlockLimitSym.hpp" +#include "BlockLowPass2.hpp" +#include "BlockLowPass.hpp" +#include "BlockLowPassVector.hpp" +#include "BlockOutput.hpp" +#include "BlockPD.hpp" +#include "BlockP.hpp" +#include "BlockPID.hpp" +#include "BlockPI.hpp" +#include "BlockRandGauss.hpp" +#include "BlockRandUniform.hpp" +#include "BlockStats.hpp"