mtecs: add FPA D gain

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -91,7 +91,7 @@ protected:
 	/* control blocks */
 	BlockFFPILimited _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
 	BlockFFPILimited _controlEnergyDistribution;	/**< FFPI controller for energy distribution control: output is pitch */
-	BlockPLimited	_controlAltitude;		/**< P controller for altitude: output is the flight path angle setpoint */
+	BlockPDLimited	_controlAltitude;		/**< P controller for altitude: output is the flight path angle setpoint */
 	BlockPLimited	_controlAirSpeed;			/**< P controller for airspeed: output is acceleration setpoint */
 
 	/* Other calculation Blocks */

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;
+};
+
 }
 

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -177,6 +177,23 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f);
  */
 PARAM_DEFINE_FLOAT(MT_FPA_P, 0.1f);
 
+/**
+ * D gain for the altitude control
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
+
+/**
+ * Lowpass for FPA error derivative (see MT_FPA_D)
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f);
+
+
 /**
  * Minimal flight path angle setpoint
  *