mtecs: first rough version of takeoff mode

4 files changed, 103 insertions, 26 deletions

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 3118ea5d1..33bfd4962 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -58,6 +58,8 @@ mTecs::mTecs() :
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
 	_pitchSp(0.0f),
+	BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
+	BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
 	timestampLastIteration(hrt_absolute_time()),
 	_firstIterationAfterReset(true),
 	dtCalculated(false),
@@ -69,7 +71,7 @@ mTecs::~mTecs()
 {
 }
 
-void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp)
+void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode)
 {
 
 	/* time measurement */
@@ -80,10 +82,10 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt
 		warnx("***");
 		warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
 	}
-	updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp);
+	updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode);
 }
 
-void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) {
+void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) {
 
 	/* time measurement */
 	updateTimeMeasurement();
@@ -92,10 +94,10 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn
 	if (_counter % 10 == 0) {
 		warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp);
 	}
-	updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp);
+	updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode);
 }
 
-void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp)
+void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode)
 {
 	/* time measurement */
 	updateTimeMeasurement();
@@ -131,12 +133,21 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh
 				(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
 	}
 
+	/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
+	BlockOutputLimiter *outputLimiterThrottle = NULL;
+	BlockOutputLimiter *outputLimiterPitch = NULL;
+	if (mode == TECS_MODE_TAKEOFF) {
+		outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
+		outputLimiterPitch = &BlockOutputLimiterTakeoffPitch;
+	}
+
 	/** update control blocks **/
 	/* update total energy rate control block */
-	_throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError);
+	_throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle);
 
 	/* update energy distribution rate control block */
-	_pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError);
+	_pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch);
+
 
 	if (_counter % 10 == 0) {
 		warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f",
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 62c4d7014..6c30a458a 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -58,20 +58,25 @@ public:
 	mTecs();
 	virtual ~mTecs();
 
+	typedef enum {
+		TECS_MODE_NORMAL,
+		TECS_MODE_TAKEOFF
+	} tecs_mode;
+
 	/*
 	 * Control in altitude setpoint and speed mode
 	 */
-	void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp);
+	void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode);
 
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
 	 */
-	void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp);
+	void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode);
 
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
 	 */
-	void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp);
+	void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode);
 
 	/*
 	 * Reset all integrators
@@ -89,8 +94,8 @@ protected:
 	control::BlockParamInt _mTecsEnabled;		/**< 1 if mTecs is enabled */
 
 	/* control blocks */
-	BlockFFPILimited _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
-	BlockFFPILimited _controlEnergyDistribution;	/**< FFPI controller for energy distribution control: output is pitch */
+	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 */
 	BlockPLimited	_controlAirSpeed;			/**< P controller for airspeed: output is acceleration setpoint */
 
@@ -101,6 +106,10 @@ protected:
 	float _throttleSp;				/**< Throttle Setpoint from 0 to 1 */
 	float _pitchSp;					/**< Pitch Setpoint from -pi to pi */
 
+	/* Output Limits in special modes */
+	BlockOutputLimiter BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */
+	BlockOutputLimiter BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */
+
 	/* Time measurements */
 	hrt_abstime timestampLastIteration;		/**< Saves the result of hrt_absolute_time() of the last iteration */
 
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 6e90a82ba..f3dc9bcb2 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -111,46 +111,64 @@ protected:
 
 typedef
 
-/* A combination of feed forward, P and I gain  using the output limiter*/
+/* A combination of feed forward, P and I gain using the output limiter*/
 class BlockFFPILimited: public SuperBlock
 {
 public:
 // methods
 	BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
 		SuperBlock(parent, name),
+		_outputLimiter(this, "", isAngularLimit),
 		_integral(this, "I"),
 		_kFF(this, "FF"),
 		_kP(this, "P"),
 		_kI(this, "I"),
-		_offset(this, "OFF"),
-		_outputLimiter(this, "", isAngularLimit)
+		_offset(this, "OFF")
 	{};
 	virtual ~BlockFFPILimited() {};
-	float update(float inputValue, float inputError) {
+	float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
+// accessors
+	BlockIntegralNoLimit &getIntegral() { return _integral; }
+	float getKFF() { return _kFF.get(); }
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	float getOffset() { return _offset.get(); }
+	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+protected:
+	BlockOutputLimiter _outputLimiter;
+
+	float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);}
+	float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) {
 		float difference = 0.0f;
 		float integralYPrevious = _integral.getY();
-		float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);
-		if(!getOutputLimiter().limit(output, difference) &&
+		float output = calcUnlimitedOutput(inputValue, inputError);
+		if(!outputLimiter.limit(output, difference) &&
 			(((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) ||
 			((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) {
 				getIntegral().setY(integralYPrevious);
 		}
 		return output;
 	}
-// accessors
-	BlockIntegralNoLimit &getIntegral() { return _integral; }
-	float getKFF() { return _kFF.get(); }
-	float getKP() { return _kP.get(); }
-	float getKI() { return _kI.get(); }
-	float getOffset() { return _offset.get(); }
-	BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
 private:
 	BlockIntegralNoLimit _integral;
 	BlockParamFloat _kFF;
 	BlockParamFloat _kP;
 	BlockParamFloat _kI;
 	BlockParamFloat _offset;
-	BlockOutputLimiter _outputLimiter;
+};
+
+/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/
+class BlockFFPILimitedCustom: public BlockFFPILimited
+{
+public:
+// methods
+	BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+		BlockFFPILimited(parent, name, isAngularLimit)
+		{};
+	virtual ~BlockFFPILimitedCustom() {};
+	float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) {
+		return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter);
+	}
 };
 
 /* A combination of P gain and output limiter */
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 f8db70304..bbd957273 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -246,3 +246,42 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
+
+
+/**
+ * Minimal throttle during takeoff
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
+
+/**
+ * Maximal throttle during takeoff
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
+
+/**
+ * Minimal pitch during takeoff
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
+
+/**
+ * Maximal pitch during takeoff
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);