From 4824484497cf0d85d08fe9c9727aa5edc4446e67 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 19:03:57 +0100 Subject: mtecs: add FPA D gain --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 48 ++++++++++++++-------- 1 file changed, 32 insertions(+), 16 deletions(-) (limited to 'src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h') 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 c15d90cdb..a357b24e2 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -87,10 +87,6 @@ public: 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(); -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", -// name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { difference = value - minimum; value = minimum; @@ -134,17 +130,11 @@ public: float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, -// (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { getIntegral().setY(integralYPrevious); } -// warnx("%s output limited %.2f", -// name,(double)output); return output; } // accessors @@ -177,13 +167,7 @@ public: float update(float input) { float difference = 0.0f; float output = getKP() * input; -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s output %.2f _kP.get() %.2f, input", -// name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference); -// warnx("%s output limited %.2f", -// name,(double)output); return output; } // accessors @@ -194,5 +178,37 @@ private: BlockOutputLimiter _outputLimiter; }; +/* A combination of P, D gains and output limiter */ +class BlockPDLimited: public SuperBlock +{ +public: +// methods + BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _kD(this, "D"), + _derivative(this, "D"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPDLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input + getKD() * getDerivative().update(input); + getOutputLimiter().limit(output, difference); + + return output; + } +// accessors + float getKP() { return _kP.get(); } + float getKD() { return _kD.get(); } + BlockDerivative &getDerivative() { return _derivative; } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +private: + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; + BlockDerivative _derivative; + BlockOutputLimiter _outputLimiter; +}; + } -- cgit v1.2.3