mtecs fix integrator

2 files changed, 22 insertions, 25 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 f3dc9bcb2..047e4d335 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -51,27 +51,6 @@ namespace fwPosctrl
 
 using namespace control;
 
-/* Integrator without limit */
-class BlockIntegralNoLimit: public SuperBlock
-{
-public:
-// methods
-	BlockIntegralNoLimit(SuperBlock *parent, const char *name) :
-		SuperBlock(parent, name),
-		_y(0) {};
-	virtual ~BlockIntegralNoLimit() {};
-	float update(float input) {
-		setY(getY() + input * getDt());
-		return getY();
-	};
-// accessors
-	float getY() {return _y;}
-	void setY(float y) {_y = y;}
-protected:
-// attributes
-	float _y; /**< previous output */
-};
-
 /* An block which can be used to limit the output */
 class BlockOutputLimiter: public SuperBlock
 {
@@ -128,7 +107,7 @@ public:
 	virtual ~BlockFFPILimited() {};
 	float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
 // accessors
-	BlockIntegralNoLimit &getIntegral() { return _integral; }
+	BlockIntegral &getIntegral() { return _integral; }
 	float getKFF() { return _kFF.get(); }
 	float getKP() { return _kP.get(); }
 	float getKI() { return _kI.get(); }
@@ -143,14 +122,14 @@ protected:
 		float integralYPrevious = _integral.getY();
 		float output = calcUnlimitedOutput(inputValue, inputError);
 		if(!outputLimiter.limit(output, difference) &&
-			(((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) ||
-			((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) {
+			(((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
+			((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
 				getIntegral().setY(integralYPrevious);
 		}
 		return output;
 	}
 private:
-	BlockIntegralNoLimit _integral;
+	BlockIntegral _integral;
 	BlockParamFloat _kFF;
 	BlockParamFloat _kP;
 	BlockParamFloat _kI;
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 3a05f2c88..591257611 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -360,3 +360,21 @@ PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f);
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f);
+
+/**
+ * Integrator Limit for Total Energy Rate Control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f);
+
+/**
+ * Integrator Limit for Energy Distribution Rate Control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f);