mtecs: add D gain for speed outer loop

src/modules/fw_pos_control_l1/mtecs/mTecs.h

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

src/modules/fw_pos_control_l1/mtecs/mTecs_params.c

@@ -233,6 +233,23 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
  */
 PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f);
 
+/**
+ * D gain for the airspeed control
+ * Maps the change of airspeed error to the acceleration setpoint
+ *
+ * @min 0.0f
+ * @max 10.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
+
+/**
+ * Lowpass for ACC error derivative calculation (see MT_ACC_D)
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
+
 /**
  * Minimal acceleration (air)
  *