simple underspeed protection for mtecs

3 files changed, 65 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 63e60995e..1fb3153e9 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -50,6 +50,7 @@ mTecs::mTecs() :
 	SuperBlock(NULL, "MT"),
 	/* Parameters */
 	_mTecsEnabled(this, "ENABLED"),
+	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* control blocks */
 	_controlTotalEnergy(this, "THR"),
 	_controlEnergyDistribution(this, "PIT", true),
@@ -58,11 +59,13 @@ mTecs::mTecs() :
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
 	_pitchSp(0.0f),
-	BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
-	BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
+	_BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
+	_BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
+	_BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"),
+	_BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true),
 	timestampLastIteration(hrt_absolute_time()),
 	_firstIterationAfterReset(true),
-	dtCalculated(false),
+	_dtCalculated(false),
 	_counter(0)
 {
 }
@@ -133,12 +136,20 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh
 				(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
 	}
 
+	/* Check airspeed: if below safe value switch to underspeed mode */
+	if (airspeed < _airspeedMin.get()) {
+		mode = TECS_MODE_UNDERSPEED;
+	}
+
 	/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
 	BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits
 	BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits
 	if (mode == TECS_MODE_TAKEOFF) {
-		outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
-		outputLimiterPitch = &BlockOutputLimiterTakeoffPitch;
+		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
+		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
+	} else if (mode == TECS_MODE_UNDERSPEED) {
+		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
+		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
 
 	/** update control blocks **/
@@ -159,7 +170,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh
 
 	/* clean up */
 	_firstIterationAfterReset = false;
-	dtCalculated = false;
+	_dtCalculated = false;
 
 	_counter++;
 }
@@ -174,7 +185,7 @@ void mTecs::resetIntegrators()
 
 void mTecs::updateTimeMeasurement()
 {
-	if (!dtCalculated) {
+	if (!_dtCalculated) {
 		float deltaTSeconds = 0.0f;
 		if (!_firstIterationAfterReset) {
 			hrt_abstime timestampNow = hrt_absolute_time();
@@ -183,7 +194,7 @@ void mTecs::updateTimeMeasurement()
 		}
 		setDt(deltaTSeconds);
 
-		dtCalculated = true;
+		_dtCalculated = true;
 	}
 }
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 6c30a458a..393dd294d 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -60,6 +60,7 @@ public:
 
 	typedef enum {
 		TECS_MODE_NORMAL,
+		TECS_MODE_UNDERSPEED,
 		TECS_MODE_TAKEOFF
 	} tecs_mode;
 
@@ -92,6 +93,7 @@ public:
 protected:
 	/* parameters */
 	control::BlockParamInt _mTecsEnabled;		/**< 1 if mTecs is enabled */
+	control::BlockParamFloat _airspeedMin;		/**< minimal airspeed */
 
 	/* control blocks */
 	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output is throttle */
@@ -107,14 +109,16 @@ protected:
 	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 */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle;		/**< Throttle Limits during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterTakeoffPitch;      	/**< Pitch Limit during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle;	/**< Throttle Limits during takeoff */
+	BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch;   	/**< Pitch Limit during takeoff */
 
 	/* Time measurements */
 	hrt_abstime timestampLastIteration;		/**< Saves the result of hrt_absolute_time() of the last iteration */
 
 	bool _firstIterationAfterReset;			/**< True during the first iteration after a reset */
-	bool dtCalculated;				/**< True if dt has been calculated in this iteration */
+	bool _dtCalculated;				/**< True if dt has been calculated in this iteration */
 
 	int _counter;
 
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 85a4f3d48..fd501c17a 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -247,7 +247,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
  */
 PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
 
-
 /**
  * Minimal throttle during takeoff
  *
@@ -285,3 +284,42 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
  * @group mTECS
  */
 PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);
+
+/**
+ * Minimal throttle in underspeed mode
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f);
+
+/**
+ * Maximal throttle in underspeed mode
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f);
+
+/**
+ * Minimal pitch in underspeed mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f);
+
+/**
+ * Maximal pitch in underspeed mode
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f);
+