mtecs: support overriding limit parameters

3 files changed, 113 insertions, 12 deletions

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 087e1b476..d370bf906 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -79,7 +79,8 @@ mTecs::~mTecs()
 {
 }
 
-int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode)
+int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
@@ -105,10 +106,12 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 
 	/* use flightpath angle setpoint for total energy control */
-	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode);
+	return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
+			airspeedSp, mode, limitOverride);
 }
 
-int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode)
+int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+		float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
@@ -135,10 +138,12 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
-	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode);
+	return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed,
+			accelerationLongitudinalSp, mode, limitOverride);
 }
 
-int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode)
+int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+		float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
 {
 	/* check if all input arguments are numbers and abort if not so */
 	if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
@@ -180,8 +185,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 				(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
 	}
 
-	/* Check airspeed: if below safe value switch to underspeed mode */
-	if (airspeed < _airspeedMin.get()) {
+	/* Check airspeed: if below safe value switch to underspeed mode (if not in takeoff mode) */
+	if (!TECS_MODE_LAND && airspeed < _airspeedMin.get()) {
 		mode = TECS_MODE_UNDERSPEED;
 	}
 
@@ -202,6 +207,16 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
 
+	/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
+	 * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
+	 * is running) */
+	bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
+			_controlTotalEnergy.getOutputLimiter() :
+			*outputLimiterThrottle,
+			outputLimiterPitch == NULL ?
+			_controlEnergyDistribution.getOutputLimiter() :
+			*outputLimiterPitch);
+
 	/* Write part of the status message */
 	_status.airspeedDerivativeSp = airspeedDerivativeSp;
 	_status.airspeedDerivative = airspeedDerivative;
@@ -280,5 +295,29 @@ void mTecs::debug(const char *fmt, ...) {
 	debug_print(fmt, args);
 }
 
-} /* namespace fwPosctrl */
+bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+		BlockOutputLimiter &outputLimiterPitch)
+{
+	bool ret = false;
 
+	if (overrideThrottleMinEnabled)	{
+		outputLimiterThrottle.setMin(overrideThrottleMin);
+		ret = true;
+	}
+	if (overrideThrottleMaxEnabled)	{
+		outputLimiterThrottle.setMax(overrideThrottleMax);
+		ret = true;
+	}
+	if (overridePitchMinEnabled)	{
+		outputLimiterPitch.setMin(overridePitchMin);
+		ret = true;
+	}
+	if (overridePitchMaxEnabled)	{
+		outputLimiterPitch.setMax(overridePitchMax);
+		ret = true;
+	}
+
+	return ret;
+}
+
+} /* namespace fwPosctrl */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 376d39698..1a787df72 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -68,20 +68,81 @@ public:
 		TECS_MODE_LAND_THROTTLELIM
 	} tecs_mode;
 
+
+	/* A small class which provides helper fucntions to override control output limits which are usually set by
+	 * parameters in special cases
+	 */
+	class LimitOverride
+	{
+	public:
+		LimitOverride() :
+			overrideThrottleMinEnabled(false),
+			overrideThrottleMaxEnabled(false),
+			overridePitchMinEnabled(false),
+			overridePitchMaxEnabled(false)
+		{};
+
+		~LimitOverride() {};
+
+		/*
+		 * Override the limits of the outputlimiter instances given by the arguments with the limits saved in
+		 * this class (if enabled)
+		 * @return true if the limit was applied
+		 */
+		bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
+				BlockOutputLimiter &outputLimiterPitch);
+
+		/* Functions to enable or disable the override */
+		void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
+				&overrideThrottleMin, value); }
+		void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
+		void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
+				&overrideThrottleMax, value); }
+		void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
+		void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
+				&overridePitchMin, value); }
+		void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
+		void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
+				&overridePitchMax, value); }
+		void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
+
+	protected:
+		bool overrideThrottleMinEnabled;
+		float overrideThrottleMin;
+		bool overrideThrottleMaxEnabled;
+		float overrideThrottleMax;
+		bool overridePitchMinEnabled;
+		float overridePitchMin; //in degrees (replaces param values)
+		bool overridePitchMaxEnabled;
+		float overridePitchMax; //in degrees (replaces param values)
+
+		/* Enable a specific limit override */
+		void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value;
+		warnx("value %.3f", value);
+		};
+		/* Disable a specific limit override */
+		void disable(bool *flag) { *flag = false; };
+
+
+	};
+
 	/*
 	 * Control in altitude setpoint and speed mode
 	 */
-	int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode);
+	int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
 	 */
-	int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode);
+	int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
+			float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
 	 */
-	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode);
+	int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed,
+			float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
 
 	/*
 	 * Reset all integrators
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 a7acd95de..e4e405227 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -89,6 +89,8 @@ public:
 	bool isAngularLimit() {return _isAngularLimit ;}
 	float getMin() { return _min.get(); }
 	float getMax() { return _max.get(); }
+	void setMin(float value) { _min.set(value); }
+	void setMax(float value) { _max.set(value); }
 protected:
 //attributes
 	bool _isAngularLimit;
@@ -96,7 +98,6 @@ protected:
 	control::BlockParamFloat _max;
 };
 
-typedef
 
 /* A combination of feed forward, P and I gain using the output limiter*/
 class BlockFFPILimited: public SuperBlock