mtecs: first rough version of takeoff mode

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -361,7 +361,7 @@ private:
 	void reset_landing_state();
 
 	/*
-	 * Call TECS
+	 * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
 	 */
 	void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
 			float pitch_min_rad, float pitch_max_rad,
@@ -1364,7 +1364,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		if (ground_speed_length > FLT_EPSILON) {
 			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
 		}
-		_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp);
+
+		if (!climbout_mode) {
+			/* Normal operation */
+			_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_NORMAL);
+		} else {
+			/* Climbout/Takeoff mode requested */
+			_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
+		}
 
 	} else {
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp,

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",

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 */
+	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
-	BlockFFPILimited _controlEnergyDistribution;	/**< FFPI controller for energy distribution control: output is pitch */
+	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 */
 

src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h

@@ -111,32 +111,22 @@ 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"),
+		_offset(this, "OFF")
-		_outputLimiter(this, "", isAngularLimit)
 	{};
 	virtual ~BlockFFPILimited() {};
-	float update(float inputValue, float inputError) {
+	float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _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) &&
-			(((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(); }
@@ -144,13 +134,41 @@ public:
 	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 = 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;
+	}
 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 */

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);