diff options
author | px4dev <px4@purgatory.org> | 2013-01-06 13:58:52 -0800 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-01-06 13:58:52 -0800 |
commit | 950d104c8d7e335b88c0a7944628c14293a0f676 (patch) | |
tree | 958eb5dc90b8000067f2f247692497d650dad822 /apps/systemlib/control/blocks.hpp | |
parent | 43ccb257a8a60a24b81396dfbf7841917db3f74e (diff) | |
parent | 8888b73e160520e5b15e168998013f4a5f6e64c0 (diff) | |
download | px4-firmware-950d104c8d7e335b88c0a7944628c14293a0f676.tar.gz px4-firmware-950d104c8d7e335b88c0a7944628c14293a0f676.tar.bz2 px4-firmware-950d104c8d7e335b88c0a7944628c14293a0f676.zip |
Merge commit '8888b73e160520e5b15e168998013f4a5f6e64c0' into local/mathlib
Diffstat (limited to 'apps/systemlib/control/blocks.hpp')
-rw-r--r-- | apps/systemlib/control/blocks.hpp | 494 |
1 files changed, 494 insertions, 0 deletions
diff --git a/apps/systemlib/control/blocks.hpp b/apps/systemlib/control/blocks.hpp new file mode 100644 index 000000000..95f2021ce --- /dev/null +++ b/apps/systemlib/control/blocks.hpp @@ -0,0 +1,494 @@ +/**************************************************************************** + * + * 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 <assert.h> +#include <time.h> +#include <stdlib.h> +#include <systemlib/test/test.hpp> + +#include "block/Block.hpp" +#include "block/BlockParam.hpp" + +namespace control +{ + +int __EXPORT basicBlocksTest(); + +/** + * 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 + BlockParam<float> _min; + BlockParam<float> _max; +}; + +int __EXPORT blockLimitTest(); + +/** + * 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 + BlockParam<float> _max; +}; + +int __EXPORT blockLimitSymTest(); + +/** + * 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), + _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; + BlockParam<float> _fCut; +}; + +int __EXPORT blockLowPassTest(); + +/** + * 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 */ + BlockParam<float> _fCut; /**< cut-off frequency, Hz */ +}; + +int __EXPORT blockHighPassTest(); + +/** + * 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 */ +}; + +int __EXPORT blockIntegralTest(); + +/** + * 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 */ +}; + +int __EXPORT blockIntegralTrapTest(); + +/** + * 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), + _lowPass(this, "LP") + {}; + virtual ~BlockDerivative() {}; + float update(float input); +// accessors + void setU(float u) {_u = u;} + float getU() {return _u;} + float getLP() {return _lowPass.getFCut();} +protected: +// attributes + float _u; /**< previous input */ + BlockLowPass _lowPass; /**< low pass filter */ +}; + +int __EXPORT blockDerivativeTest(); + +/** + * 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: + BlockParam<float> _kP; +}; + +int __EXPORT blockPTest(); + +/** + * 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; + BlockParam<float> _kP; + BlockParam<float> _kI; +}; + +int __EXPORT blockPITest(); + +/** + * 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; + BlockParam<float> _kP; + BlockParam<float> _kD; +}; + +int __EXPORT blockPDTest(); + +/** + * 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; + BlockParam<float> _kP; + BlockParam<float> _kI; + BlockParam<float> _kD; +}; + +int __EXPORT blockPIDTest(); + +/** + * 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 + BlockParam<float> _trim; + BlockLimit _limit; + float _val; +}; + +int __EXPORT blockOutputTest(); + +/** + * 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 = MAX_RAND; + 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 + BlockParam<float> _min; + BlockParam<float> _max; +}; + +int __EXPORT blockRandUniformTest(); + +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() / MAX_RAND; + float U2 = (float)rand() / MAX_RAND; + V1 = 2 * U1 - 1; + V2 = 2 * U2 - 1; + S = V1 * V1 + V2 * V2; + } while (S >= 1 || fabsf(S) < 1e-8f); + + X = V1 * float(sqrt(-2 * float(log(S)) / S)); + + } else + X = V2 * float(sqrt(-2 * float(log(S)) / S)); + + phase = 1 - phase; + return X * getStdDev() + getMean(); + } +// accessors + float getMean() { return _mean.get(); } + float getStdDev() { return _stdDev.get(); } +private: +// attributes + BlockParam<float> _mean; + BlockParam<float> _stdDev; +}; + +int __EXPORT blockRandGaussTest(); + +} // namespace control |