diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-24 14:07:51 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-24 14:07:51 +0200 |
commit | 2a354c2d10a49c65a05665a0d631ebe615f306bc (patch) | |
tree | 3ea34a32356ec634038f3f869ad072e6f1202f7c | |
parent | c00eaaf295b1804181a724134115f299fd0ad399 (diff) | |
download | px4-firmware-2a354c2d10a49c65a05665a0d631ebe615f306bc.tar.gz px4-firmware-2a354c2d10a49c65a05665a0d631ebe615f306bc.tar.bz2 px4-firmware-2a354c2d10a49c65a05665a0d631ebe615f306bc.zip |
mtecs: fix usage of outputLimiter.limit
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 10 |
1 files changed, 9 insertions, 1 deletions
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index 047e4d335..a7acd95de 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -63,6 +63,14 @@ public: _max(this, "MAX") {}; virtual ~BlockOutputLimiter() {}; + /* + * Imposes the limits given by _min and _max on value + * + * @param value is changed to be on the interval _min to _max + * @param difference if the value is changed this corresponds to the change of value * (-1) + * otherwise unchanged + * @return: true if the limit is applied, false otherwise + */ bool limit(float& value, float& difference) { float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); @@ -121,7 +129,7 @@ protected: float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = calcUnlimitedOutput(inputValue, inputError); - if(!outputLimiter.limit(output, difference) && + if(outputLimiter.limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { getIntegral().setY(integralYPrevious); |